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PREFACE 


This  analysis  was  conducted  and  this  report  was  prepared  in 
conjunction  with  the  Robotic  Refueler  software  programs 
being  developed  by  Analytical  and  Physical  Simulation  Branch 
at  the  U.S.  Army  Tank-Automotive  Command  and  to  fulfill  the 
requirements  for  a  final  class  project  in  the  class  "Special 
Topics  in  Robotics  (EGR  595)"  taken  during  the  fall  1988 
semester  at  Oakland  University  located  in  Rochester  Michigan 
The  author's  attendance  in  this  class  was  supported  by  the 
U.S.  Army  Tank-Automotive  Command.  The  class  instructor  was 
You-Liang  Gu  from  the  Department  of  Electrical  and  Systems 
Engineering. 

The  author  wishes  to  acknowledge  and  thank  Professor 
You-Liang  Gu  for  the  instruction  and  assistance  he  provided 
on  this  project  and  for  the  good  grade  recieved. 

Since  most  of  the  work  in  preparing  this  report  was 
conducted  at  home,  the  author  would  like  to  dedicate  this 
report  to  his  wife  Donna,  who  was  very  patient  and 
supportive  during  these  studies. 
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1.0.  INTRODUCTION 


Currently  the  U.S.  Army  is  engaged  in  incorporating 
adavanced  robotics  into  military  vehicle  systems.  One 
aspect  of  robotics  used  in  military  vehicles  includes  the 
use  of  a  robotic  arm  to  perform  tasks  a  human  operator  is 
unable  to  perform  or  to  perform  tasks  which  may  endanger  the 
operator. 

One  application  for  a  robotic  arm  was  developed  by  the  U.S. 
Army  Tank  Automotive  Command  (TACOM)  which  initiated  a 
project  to  determine  the  feasibility  of  remotely  refueling 
combat  vehicles  in  the  forward  battle  area  at  a  high  flow 
rate  while  providing  crew  protection  against  ballistic  and 
Nuclear,  Biological,  and  Chemical  (NBC)  hazards.  To 
accomplish  these  goals  a  robotic  refueler  arm,  guided  either 
by  a  computer  or  by  an  operator  with  a  joystick,  is  being 
developed  and  integrated  into  a  Refueler  Demonstrator  (RD) 
vehicle. 

Another  application  for  a  robotic  arm  is  being  considered 
under  the  Heavy  Forces  Modernization  Program.  Under  this 
program  a  Combat  Mobility  Vehicle  (CMV)  is  being  proposed  to 
demonstrate  the  applicability  and  integration  of  advanced 
vehicle  and  robotic  technologies.  One  of  the  advanced 
technologies  to  be  incorporated  into  the  CMV  includes  a 
multi-use  robotic  excavating  arm.  The  robotic  excavating 
arm  will  allow  the  CMV  to  defeat  complex  obstacle  systems  or 
contruct  defensive  positions  with  improved  vehicle 
performance  and  crew  survivability. 

Both  the  robotic  refueler  arm  and  the  multi-use  robotic 
excavating  arm  require  the  integration  of  advanced  robotic 
kinematics,  dynamics,  controls,  and  sensing  with  the  vehicle 
and  crew. 

Although  this  paper  uses  the  RD  vehicle  for  the  analysis, 
the  technology  presented  can  be  applied  to  the  multi-use 
robotic  excavating  arm  or  any  robotic  arm. 

This  analysis  was  conducted  and  this  report  was  prepared  in 
conjunction  with  the  Robotic  Refueler  software  programs 
being  developed  by  Analytical  and  Physical  Simulation  Branch 
at  the  U.S.  Army  Tank-Automotive  Command  and  to  fulfill  the 
requirements  for  a  final  class  project  in  the  class  "Special 
Topics  in  Robotics  (EGR  595)"  taken  during  the  fall  1988 
semester  at  Oakland  University  located  in  Rochester 
Michigan.  The  author's  attendance  in  this  class  was 
supported  by  the  U.S.  Army  Tank -Automotive  Command.  The 
class  instructor  was  You-Liang  Gu  from  the  Department  of 
Electrical  and  Systems  Engineering. 


2.0.  OBJECTIVES 


The  objective  of  this  analysis  was  to  apply  the  theory  and 
procedures  developed  in  the  above  class  to  the  robotic  arm 
on  the  RD  vehicle.  For  the  robotic  refueler  arm,  this  study 
analyzed  and  developed  a  computer  simulation  program  to 
calculate  the  following: 

•  Kinematics 

•  Inverse  Kinematics 

•  Dynamics 

•  Global  Position  Control  Stategy 


3.0.  CONCLUSIONS 

The  robotic  refueler  has  4  degrees  of  freedom.  The  four 
degrees  of  freedom  result  from  the  four  joint  angles  and  can 
be  represented  by  the  arm's  ability  to  control  the  nozzle 
end  position  in  the  global  X,  Y,  and  Z  directions  and  the 
nozzle  approach  angle.  The  nozzle  approach  angle  is  defined 
as  the  angle  from  horizontal  of  the  nozzle  in  the  robotic 
arm's  plane  of  motion.  Since  the  arm  has  only  4  degrees  of 
freedom,  the  nozzle  cannot  be  orientated  outside  the  robotic 
arm's  plane  of  motion.  As  a  result,  the  nozzle  can  not  be 
aligned  with  all  receiver  orientations. 

To  improve  the  robotic  refueler  arm's  ability  to  align  the 
nozzle  with  all  receiver  orientations,  another  degree  of^ 
freedom  must  by  built  into  the  robotic  refueler  arm.  This 
can  be  accomplished  by  adding  another  revolute  joint  between 
the  elbow  joint  and  the  wrist  joint.  However,  if  only  small 
angles  outside  the  robotic  arm's  plane  of  motion  are 
expected,  then  a  compliant  device  connected  between  the 
nozzle  and  the  wrist  joint  could  be  used. 

A  computer  program  was  written  in  the  "C"  language  to 
simulate  the  robotic  refueler  arm's  kinematic,  dynamic,  and 
control  equations  and  to  evaluate  the  performance  of  the 
robot  control  system  against  disturbances. 

A  desired  trajectory,  which  the  controlled  robot  is  to 
follow,  was  chosen  to  be  a  72-inch  radius  circular  path  in 
the  horizontal  plane  with  a  simultaneous  48-inch  sinusoidal 
rise  in  elevation  while  maintaining  a  constant  approach 
angle  of  90  degrees.  The  initial  conditions  in  the  global 
X,  Y,  and  Z  direction  were  purposely  offset  from  the  desired 
trajectory  by  12.0  inches  and  the  nozzle  orientation  was 
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offset  by  12  degrees  to  evaluate  the  performance  of  the 
control  system  against  disturbances. 

The  simulation  results  show  that  the  position  error  in  the 
global  X,  Y,  and  Z  direction  and  the  orientation  error  goes 
to  zero  and  the  nozzle  end  follows  the  desired  tractory. 


4.0.  RECOMMENDATIONS 

In  the  future,  further  analysis  should  be  performed  on: 

•  System  hydrualics. 

•  Incorporation  of  system  hydraulics  into  a  global 
positional  control  system. 

•  Positional  controller  for  a  single  hydraulically 
driven  joint. 

•  Trajectory  planning  and  obstacle  avoidance 

•  Camera  model  to  characterize  the  formation  of  an 
image  via  the  projection  of  3D  points  onto  an  image 
plane . 

•  Integration  of  camera  and  other  sensors  with 
controller  and  with  the  vehicle  and  crew. 


5.0.  DISCUSSION 

5.1.  Combat  Mobility  Vehicle 

5.1.1.  Background.  Under  the  Heavy  Forces  Modernization 
Program,  a  Combat  Mobility  Vehicle  (CMV)  is  being  proposed 
to  demonstrate  the  applicability  and  integration  of  advanced 
vehicle  and  robotic  technologies.  Advanced  technologies 
incorporated  into  the  CMV  include:  multi-use  robotic 
excavating  arm,  mine  clearing  plow/blade,  hydraulic 
technology,  advanced  integration  propulsion  system, 

VETRONICS  (vehicle  electronics) ,  advanced  track  and 
suspension  system,  and  countermeasure  technologies.  These 
technologies  will  allow  the  CMV  to  defeat  complex  obstacle 
systems  with  improved  vehicle  and  crew  survivability,  and 
with  improved  mobility  comparable  to  other  combat  vehicles. 
Figure  5-1  shows  a  concept  drawing  of  the  CMV. 

The  multi-use  robotic  excavating  arm  requires  the 
integration  of  advanced  robotic  kinematics,  dynamics, 
controls,  and  sensing  with  the  vehicle  and  crew.  Although 
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this  paper  uses  the  Refueler  Demonstrator  vehicle  for  this 
analysis,  the  technology  presented  can  be  applied  to  any 
robotic  arm. 

5.2.  Refueler  Demonstrator  Vehicle 

5.2.1.  Background.  "During  the  development  of  the  Armored 
Resupply  Multipurpose  System  (ARMS)  program,  a  major 
requirement  evolved  for  forward  area  refueling.  The  ARMS 
solution  was  to  use  a  conventional  delivery  system  placed  on 
an  armored  tracked  carrier  to  provide  increased  mobility  and 
armor  protection.  Looking  beyond  ARMS,  the  U.S.  Army  Tank 
Automotive  Command  (TACOM)  developed  a  project  to  determine 
the  feasibility  of  remotely  refueling  combat  vehicles  in  the 
forward  battle  area.  To  achieve  this,  a  Refueler 
Demonstrator  (RD)  was  developed  for  engineering  test  and 
user  evaluation.  The  RD  project  is  being  fully  coordinated 
with  the  Belvoir  Research,  Development,  and  Engineering 
Center  (BRDEC) .  The  RD  is  also  being  used  to  evaluate  the 
Standard  Army  Refueling  System  (SARS) .  Under  this  program, 

a  number  of  interfaces  such  as  nozzles,  receptacles,  and 
fuel  flow  rates  will  be  evaluated.  Currently,  the  RD  is 
viewed  as  a  technology  brassboard  and  early-on  proof  of 
principle  for  the  refueler  variant  of  the  Future  Armored 
Resupply  Vehicle  (FARV)  in  the  armored  family  of  vehicles 
(AFV)  (under  the  Heavy  Forces  Modernization  Program) . "1 

5.2.2.  Vehicle  Description. 

5. 2. 2.1.  System  Description.  "The  RD  consists  of  a 
refueler  module  mounted  on  an  M993  track  vehicle  chassis. 
(The  RD  is  designed  to  refuel  combat  vehicles  at  a  high  flow 
rate  and  provide  crew  protection  against  ballistic  and 
Nuclear  Biological  and  Chemical  (NBC)  hazards) .  The  system 
is  shown  in  Figure  5-1.  The  fuel  delivery  system  consists 
of  the  pumping  components  for  automatic  and  manual  remote 
refueling  as  well  as  a  standard  refueling  system  that 
incorporates  a  hose  and  reel.  The  automatic  and  manual 
remote  system  has  a  variable  (fuel  delivery)  rate  adjustment 
from  1  to  250  gallons  per  minute  (GPM) .  The  standard 
refueling  system  has  an  adjustable  (fuel  delivery)  rate  from 
1  to  50  GPM.  (In  addition  to  refueling,  the  refueler  is 
capable  of  defueling  at  a  rate  of  approximately  50  GPM.) 

For  remote  refueling,  the  flow  rate,  fuel  quantity,  location 
of  the  vehicle  to  be  refueled  in  relation  to  the  RD  (left  or 
right)  is  entered  into  the  computer  by  pressing  a  series  of 
CRT  prompt  buttons.  The  operator  is  then  asked  whether  the 
remote  automatic  or  remote  manual  mode  of  operation  is  to  be 
used.  When  the  remote  automatic  mode  of  operation  is 
selected,  the  refueling  arm  will  automatically  search  for 
and  locate  the  fuel  receptacle  on  the  vehicle  to  be 
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refilled.  Location  of  the  receptacle  is  accomplished  by  the 
refueling  arm  nozzle  sensing  infrared  light  beams  which  come 
from  a  device  attached  to  the  fuel  receptacle.  The  angle  of 
the  nozzle  is  checked  and  aligned  with  the  receiving 
vehicle.  Text  from  the  computer  will  then  ask  if  the 
operator  wants  to  guide  the  refueling  arm  into  the 
receptacle  manually,  with  a  joy  stick,  or  if  the  operator 
wants  the  remote  refueling  arm  to  automatically  engage  the 
receptacle.  After  pressing  the  proper  CRT  prompt  button, 
the  fuel  receptacle  is  engaged.  When  the  remote  manual  mode 
operation  is  selected,  the  refueling  arm  search  and 
engagement  process  is  totally  controlled  by  means  of  a 
joystick."2 

5. 2. 2. 2.  Refueler  Module.  "The  refueler  module  is  armor 
protected  and  houses  a  1,500  gallon  fuel  tank,  fuel  delivery 
system,  and  self-contained  auxiliary  power  unit  (APU) . 
Armored  access  doors  are  incorporated  in  both  the  pumping 
and  APU  compartments  to  provide  access  for  service  and 
maintenance.  The  fuel  tank  is  integral  with  the  armored 
module  structure  and  includes  and  explosion  suppressing 
material  which  also  acts  as  a  baffle."3 

5. 2. 2. 3.  Cameras.  "The  RD  contains  three  cameras.  A 
camera  is  mounted  on  each  side  of  the  (refueler)  module  as 
part  of  the  observation  system  to  aid  the  operator  in 
locating  the  fuel  receptacle  of  the  receiving  vehicle  during 
automatic  and  manual  remote  operations.  These  cameras  are 
fixed  facing  rearward  and  have  a  wide  field  of  view.  A 
single  narrow  field  of  view  camera  is  also  mounted  on  the 
remote  refueling  arm  to  provide  the  operator  with  close  in 
viewing  for  nozzle  positioning."4 

5. 2. 2. 4.  Remote  Refueling  Arm.  "The  remote  refueling  arm 
is  mounted  on  the  refueler  module  and  provides  for  fuel 
delivery  from  the  module  to  the  receiving  vehicle.  It  is 
constructed  of  armor  tubing  and  can  be  automatically  or 
manually  operated  throughout  its  deployment  and  receptacle 
engagement.  The  arm  is  hydraulically  powered  and  controlled 
by  electro-hydraulic  actuators.  Signals  to  the  actuators 
are  transmitted  from  the  control  console  at  the  operator's 
control  station.  Transmitters/receivers  are  also  mounted  on 
the  arm  to  provide  the  operator  the  necessary  signals  to 
locate  the  fuel  receptacle  and  engage  the  refueling  arm 
nozzle . "5 

5. 2. 2. 5.  Control  Console.  "The  control  console  is  in  the 
vehicle  cab  between  the  driver  and  passenger  seats.  The 
control  console  consists  of  a  computer,  control  cabinet 
containing  a  CRT  monitor,  joystick,  and  associated  controls 
for  performing  remote  refueling  functions."6 
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5.3.  Kinematics 


5.3.1.  Links,  Joints,  and  Their  Parameters.  A  mechanical 
manipulator  consists  of  a  sequence  of  rigid  bodies,  called 
links,  connected  by  either  revolute  or  prismatic  joints. 

Each  joint-link  pair  constitutes  1  degree  of  freedom. 

Hence,  for  the  robotic  refueler  arm  on  the  RD  vehicle,  there 
are  4  degrees  of  freedom  as  a  result  of  the  4  revolute 
joints  between  the  4  bodies.  Figure  5-2  shows  a  drawing  of 
the  refueler  arm.  The  4  links  are:  the  rotating  base;  the 
aft  arm;  the  fore  arm;  and  the  nozzle.  The  4  joints  are: 
the  waist  joint  between  the  vehicle  chassis  and  the  rotating 
base;  the  shoulder  joint  between  the  rotating  base  and  the 
aft  arm;  the  elbow  joint  between  the  aft  arm  and  the  fore 
arm;  the  wrist  joint  between  the  fore  arm  and  the  nozzle. 

There  are  two  parameters  for  each  joint,  Joint  Offset  (di) 
and  Joint  Angle  (0. )  ,  which  determines  the  relative  position 
of  neighboring  links.  Joint  Offset  is  the  relative  position 
of  2  connected  links  (link  i-1  and  link  i)  which  is  the 
distance  along  the  joint  axis  between  normals.  Joint  Angle 
is  the  angle  between  the  normals  measured  in  a  plane  normal 
to  the  joint  axis. 

There  are  two  parameters  for  each  link.  Link  Length  (a^  and 
Twist  Angle  (o^) ,  which  determines  the  structure  of  the 
link.  Link  Length  is  the  shortest  distance  measured  along 
the  common  normal  between  joint  axes.  Twist  Angle  is  the 
angle  between  the  joint  axes  in  a  plane  perpendicular  to  the 
Link  Length. 

For  a  more  complete  description  on  each  parameter  the  reader 
should  refer  to  chapter  2  of  the  book  "Robotics:  Control, 
Sensing,  Vision,  and  Intelligence"  by  Fu,  Gonzalez,  and  Lee. 

5.3.2.  Denavit-Hartenberg  (D-H)  Representation.  "To 
describe  the  translational  and  rotational  relationships 
between  adjacent  links,  Denavit  and  Hartenberg  proposed  a 
matrix  method  of  systematically  establishing  a  coordinate 
system  (body  attached  frame)  to  each  link  of  an  articulated 
chain.  The  Denavit-Hartenberg  representation  results  in  a  4 
by  4  homogenous  transformation  matrix  representing  each 
link's  coordinate  system  at  the  joint  with  respect  to  the 
previous  link's  coordinate  system.  Thus,  through  sequential 
transformations,  the  end-effector  (nozzle)  expressed  in 
nozzle  coordinates  can  be  transformed  and  expressed  in  the 
global  (inertial,  base,  or  vehicle)  coordinates  which  make 
up  the  inertial  frame  of  the  dynamic  system."7 

"The  homogenous  transformation  matrix  is  a  4  by  4  matrix 
which  maps  a  position  vector  expressed  in  homogenous 
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AOINT  3 
elbow 


Variable 


coordinates  from  one  coordinate  system  to  another  coordinate 
system.  A  homogenous  transformation  matrix  can  be 
considered  to  consist  of  four  submatrices: 


A  = 


R3x3  P3xl 
^1x3 


A 


•  • 

I  n  s  a  p  | 

I  0  0  0  1  | 


rotation 

1 

position 

matrix 

1 

vector 

perspective 

transformation 

1 

1 

scaling 

The  upper  left  3  by  3  submatrix  represents  the  rotation 
matrix;  the  upper  right  3  by  1  submatrix  represents  the 
position  vector  of  the  origin  of  the  rotated  coordinate 
system  with  respect  to  the  reference  system;  the  lower  left 
1  by  3  submatrix  represents  perspective  transformation;  and 
the  fourth  diagonal  element  is  the  global  scaling  factor."8 

"Since  the  inverse  of  an  orthonormal  rotation  submatrix  is 
equivalent  to  its  transpose,  the  row  vectors  of  a  rotation 
submatrix  represent  the  principal  axes  of  the  reference 
coordinate  system  with  respect  to  the  rotated  coordinate 
system.  However,  the  inverse  of  a  non-orthogonal 
homogeneous  transformation  matrix  is  not  equivalent  to  its 
transpose.  The  position  of  the  origin  of  the  reference 
coordinate  system  with  respect  to  the  rotated  coordinate 
system  can  only  be  found  after  the  inverse  of  the 
homogeneous  transformation  matrix  is  determined.  In  general 
the  inverse  transformation  matrix  can  be  found  to  be: 


nx 

ny 

nz 
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sy 
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ax 

az 

-axp 
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The  column  vectors  of  the  inverse  of  a  homogenous 
transformation  matrix  represent  the  principal  axes  of  the 
reference  system  with  respect  to  the  rotated  coordinate 
system,  and  the  upper  right  3  by  1  submatrix  represents  the 
position  of  the  orgin  of  the  reference  frame  with  respect  to 
the  rotated  system."9 

The  D-H  reprsentat ion  of  a  rigid  link  is  based  upon  a  set  of 
rules  and  upon  the  four  geometric  paramters  mentioned 
previously.  The  reader  should  refer  to  the  book  "Robotics: 
Control,  Sensing,  Vision,  and  Intelligence"  by  Fu,  Gonzalez, 
and  Lee  for  the  complete  set  of  rules  for  establishing  each 
link's  coordinate  system.  The  D-H  reprsentat ion  for  the 
robotic  refueler  arm  is  given  in  Figure  5-2. 

For  a  revolute  joint,  the  homogenous  transformation  matrix 
relating  the  ith  coordinate  frame  to  the  (i-l)th  coordinate 
frame  where  0^©^,  a^  and  d^^  are  the  link  and  joint 
parameters  of  the  ith  system  and  0-  is  a  variable  is  given 
by: 


i-l 


Ai 


COS0j^ 

sin0i 

0 

0 


-coso*i*sin0i 

coso(i*cos0i 

sine^ 

0 


sino<i*sin0i  ai*cos0i 
-sinO(i*cos0i  ai*sin0i 


coso^ 

0 


di 

1 


For  a  prismatic  (translational)  joint,  the  homogenous 
transformation  matrix  relating  the  ith  coordinate  frame  to 
the  (i-l)th  coordinate  frame  where  0A,  if  a^  and  dA  are 
the  link  and  joint  parameters  of  the  ith  system  and  d^  is  a 
variable  is  given  by: 


i-l 


Ai  = 


COS0i 


sin0i 


0 

0 


-coso(i*sin0i 

coso<i*cos0i 

sino^ 

0 


sino<i*sin0i  0 
-sinoti*cos0i  0 
coso^ 

0 


di 


For  the  robotic  refueler  arm  shown  in  Figure  5-2,  the 
homogenous  transformation  matrix  relating  coordinate  system 
1  to  coordinate  system  0  is  given  by: 


18 


0 


cos01  0  - s in01 

sin01  0  cos01  0 

0  -1  0  11.25 

0  0  0  1 


The  homogenous  transformation  matrix  relating  coordinate 
system  2  to  coordinate  system  1  is  given  by: 


cos02  -sin02  0  72*cos02 

sin02  cos02  0  72*sin02 

0  0  10 

0  0  0  1 


The  homogenous  transformation  matrix  relating  coordinate 
system  3  to  coordinate  system  2  is  given  by: 


cos03  -sin03  0  72  *cos03 

sin03  cos03  0  72*sin03 

0  0  10 

0  0  0  1 


The  homogenous  transformation  matrix  relating  coordinate 
system  4  to  coordinate  system  3  is  given  by: 


cos04  -sin04  0  36*cos04 


sin0, 


cos04  0  36*sin04 


5.3.3.  Position.  The  kinematic  position  analysis 
calculates,  in  the  Cartesian  space,  the  position  of  the 
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nozzle  (coordinate  system  4)  with  respect  to  the  inertial 
system  (X4/0,  Y4/0,  Z4/0)  given  the  joint  angles  0lf  02,  03, 
and  04 . 

The  homogenous  matrix  °Ti,  which  specifies  the  location  of 
the  ith  coordinate  system  with  respect  to  the  inertial 
coordinate  system  located  on  the  vehicle,  is  the  chain 
product  of  successive  coordinate  transformation  matrices 
and  is  expressed  as: 


0 


i-ii 


for  i  =  l,2,...,n  where  n  is  the  number  of  links. 


The  homogenous  transformation  matrix  relating  coordinate 
system  2  to  coordinate  system  0  is  given  by: 


clc2 

-cls2 

-si 

72*clc2 

slc2 

-sls2 

cl 

72*slc2 

-s2 

-c2 

0 

-72*s2+ll .25 

0 

0 

0 

1 

where  cl  =  Cos(01);  c2  =  Cos(02); 
si  =  Sin(01);  s2  =  Sin(02). 

The  homogenous  transformation  matrix  relating  coordinate 
system  3  to  coordinate  system  0  is  given  by: 


• 

I  clc2c3-cls2s3  -clc2s3-cls2c3  -si  72*  (clc2c3- 
|  cls2s3+clc2) 

I  Slc2c3-sls2s3  -slc2s3  +  sls2c3  cl  72*  (slc2c3- 
|  sls2s3+slc2) 


-s2c3-c2s3 


s2s3-c2c3  0  -72*  (s2c3-c2s3-s2) 

+11.25 


0 


0  0 


1 


where  c3  =  Cos  (03)  ;  s3  =  Sin(03). 
This  can  be  simplified  to: 


20 


clc23  -cls23  -si  72*clc23+72*clc2)  | 

slc23  -sls23  cl  72*slc23+72*slc2)  | 

-s23  -c23  0  -72*s23-72*s2+ll . 25  | 

0  0  0  1  | 

where : 

c23  =  cos(02+03)  =  cos  (02)  cos  (03) -sin  (02)  sin  (03) 
s23  =  sin (02+03)  =  sin (02) cos (03) +cos (02)  sin (03) 

The  homogenous  transformation  matrix  relating  coordinate 
system  4  to  coordinate  system  0  is  given  by: 


where : 


clc234 

slc234 

-s234 

0 

c234  = 


-cls234  -si 
-sls234  cl 
-c234  0 

0  0 


(36*clc234  +  72*clc23  +  72*clc2) 
(36*slc234  +  72*slc23  +  72*slc2) 
(-36s234  -  72*s23  -  72*s2  +  11.25) 
1 


cos  (02+03+04)  and  s234  =  sin  (02+03+04) 


The  position  of  the  nozzle  (coordinate  system  4)  with 
respect  to  the  inertial  system  (x4/0,  y4/0,  z4/0)  ,  given  the 
joint  angles  01#  02,  03,  and  04,  is  found  by  extracting  the 
position  vector  from  the  transformation  matrix  °T4 .  The 
approach  angle  (0a)  of  the  nozzle  with  respect  to  the 
receiver,  which  is  more  clearly  defined  in  the  Inverse 
Kinematics  section,  is  calculated  by:  0a  =  02  +  03  +  04. 

The  position  and  orientation  of  the  nozzle  in  Cartesian 
space  is  a  function  of  the  joint  angles  and  can  be  written 
as: 


y  =  Mq) 

where  y  is  the  position  vector  in  Cartesian  Space 


y  = 


X4/0 

^4/0 

^4/0 

0= 
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and  where  q  is  the  joint  position  vector  of  variables  in 
Joint  Space 


q  = 


% 

02 

0- 

0 : 


The  position  of  the  nozzle  (coordinate  system  4)  in 
cartesian  coordinates  given  the  joint  angles  is  equal  to: 


1  K4/0  I 

|  36*clc234 

+ 

72*clc23 

+ 

72*clc2  I 

y4  /o  1  = 

|  36*slc234 

+ 

72*slc23 

+ 

72*slc2  1 

1  z4/0  ' 

I  -36s234 

- 

72*s23 

- 

72*s2  +  11.25  | 

!_■  _! 

1  02 

+ 

03 

+ 

04 

5.3.4.  Velocity.  The  kinematic  velocity  analysis 
calculates  the  velocity  of  the  nozzle  (coordinate  system  4) 
with  respect  to  the  inertial  system  given  the  joint  angles 
and  joint  velocities  using: 

y  =  (Ah/Aq)  q  =  Jh  q 

where  Jh  is  the  Jacobian  matrix  defined  by: 


4hx 

Ahi 

Ah^ 

W 

^4 

w 

1^2 

Ah2 

TS? 

^3 

^h3 

^h3 

^2 

^h4 

ih* 

^2 

^■h4 

TT3 

^h4 

For  the  robotic  refueler  arm,  the  first  row  of  the  Jacobian 
matrix  is: 

=  -  36slc234  -  72slc23  -  72slc2 
=  -  36cls234  -  72cls23  -  72cls2 


Jh  (1, 1) 
Jh(l,  2) 
Jh(l,  3) 


36cls234 


72cls23 
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Jh (1, 4)  =  -  36cls234 

The  second  row  of  the  Jacobian  matrix  is: 

Jh  (2, 1)  =  36clc234  +  72clc23  +  72clc2 

Jh(2,2)  =  -  36sls234  -  72sls23  -  72sls2 
Jh (2, 3)  =  -  36sls234  -  72sls23 
Jh  (2, 4)  =  -  36sls234 

The  third  row  of  the  Jacobian  matrix  is: 

Jh ( 3 , 1 )  =  0 

Jh (3, 2)  =  -  36c234  -  72c23  -  72c2 
Jh (3, 3)  =  -  36c234  -  72c23 
Jh (3, 4 )  =  -  36c234 

The  fourth  row  of  the  Jacobian  matrix  is: 

Jh  (4, 1)  =  0 

Jh (4/2)  =  1 

Jh  (4,  3)  =  1 

Jh (4, 4)  =  1 

5.3.5.  Acceleration.  The  kinematic  acceleration  analysis 
calculates  the  acceleration  of  the  nozzle  (coordinate 
system  4)  with  respect  to  the  inertial  system  given  the 
joint  angles,  joint  velocities,  joint  accelerations,  and  the 
time  derivative  of  the  Jacobian  matrix  using: 

*•  *  9  •  • 

y  =  Jh  q  +  Jh  q 

The  time  derivative  of  the  Jacobian  matrix,  denoted  by  Jhd, 
for  the  robotic  refueler  arm  is  given  below: 

The  first  row: 

Jhd (1,1)  =  -  36clc234  *  0.  +  36sls234  *  (&2+S§3+04) 

-  72clc23  *  &1  +  72sls23  *  (02+£$3) 

72clc2  *  bx  +  72sls2  *  (b2) 

Jhd (1,2)  =  +  36sls234  *  0X  -  36clc234  *  (b2+b3+bA) 

+  72sls23  *  bx  -  72clc23  *  (02+03) 

+  72sls2  *  -  72clc2  *  (02) 
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Jhd(l,3)  =  +  36sls234  *  01 

+  72sls23  *  &1 

Jhd (1,4)  =  +  36sls234  *  01 

The  second  row: 

Jhd (2,1)  =  -  36slc234  *  0X 

-  72slc23  *  &1 

72slc2  *  0X 

Jhd (2,2)  =  -  36cls234  *  0X 

-  72cls23  *  01 

72cls2  *  0X 

Jhd (2,3)  =  -  36cls234  *  &x 

-  72cls23  *  01 

Jhd (2,4)  =  -  36cls234  *  0X 
The  third  row: 

Jhd (3,1)  =  0 

Jhd (3,2)  =  36s234  *  (02+03+04)  +  72s23  *  (02+03)  +  72s2  *  (0 
Jhd  (3,3)  =  36s234  *  <02+03+04)  +  72s23  *  (02+0*3) 

Jhd (3,4)  =  36s234  *  (02+03+04) 

The  fourth  row: 

Jhd (4,1)  =  0 

Jhd (4,2)  =  0 

Jhd (4,3)  =  0 

Jhd (4,4)  =  0 

5.4.  Inverse  Kinematics 

5.4.1.  Joint  Angles  -  A  Geometric  Approach.  The  inverse 
kinematic  position  analysis  calculates  the  joint  angles 
0lt  02,  03,  and  04  given  the  desired  position  and 
orientation  of  the  nozzle  with  respect  to  the  global  system 

An  example  of  an  inverse  kinematic  problem  is  to  find  the 
joint  angles  necessary  to  position  and  orientate  the  nozzle 
into  the  receiver.  This  requires  that  the  position  and 


-  36clc234  *  (02+03+04) 

-  72clc23  *  <&2+$3) 

-  36clc234  *  (02+03+04) 


-  36cls234  *  (02+03+04) 

-  72cls23  *  (f§2+03) 

72cls2  *  (b2) 

-  36slc234  *  (02+03+04) 

-  72slc23  *  (02+03) 

72slc2  *  (02) 

-  36slc234  *  (02+(§3+0*4) 

-  72slc23  *  (02+S3) 

-  36slc234  *  (02+03+04) 


orientation  of  the  receiver  be  known  with  respect  to  the 
global  coordinate  system.  A  coordinate  system  is  attached 
to  the  receiver  as  shown  in  Figure  5-3  with  the  Zr  axis 
pointing  out  of  the  receiver.  The  rotation  matrix  °Ar  must 
be  calculated  from  the  receiver  orientation. 

With  the  assistance  of  Figure  5-3,  the  waist  joint  angle 
(0X )  of  the  base  with  respect  to  the  vehicle  can  easily  be 
determined  from  the  global  x  and  y  position  of  the  receiver. 

Before  the  other  joint  angles  can  be  determined,  it  is 
neccessary  to  determine  the  approach  angle  of  the  nozzle. 

The  nozzle  approach  angle  is  the  angle  from  the  horizontal 
plane  neccessary  to  put  the  nozzle  directly  into  the 
receiver.  As  shown  in  Figure  5-3,  the  receiver  centerline 
is  orientated  along  the  -Zr  direction.  The  Zr  vector  is 
projected  onto  the  X-^  Y1  plane  since  the  nozzle  orientation 
is  constrained  to  move  in  this  plane.  The  transformation 
matrix  °Ar  relating  the  orientation  of  the  receiver  to  the 
global  coordinate  system  must  by  known  and  calculated. 

After  the  approach  angle  is  known,  calculate  the  position  of 
coordinate  system  3  with  respect  to  coordinate  system  0  and 
also  calculate  the  position  of  coordinate  system  3  with 
respect  to  coordinate  system  1. 

The  shoulder  joint  angle  (02)  can  be  calculated  as  shown  in 
Figure  5-3  using  trigometric  functions  and  relationships. 

The  elbow  joint  angle  (03)  can  be  calculated  as  shown  in 
Figure  5-3  using  trigometric  functions  and  relationships. 

The  wrist  joint  angle  (04)  can  easily  be  determined  as  shown 
in  Figure  5-3. 

5.4.2.  Joint  Velocities.  The  inverse  kinematic  velocity 
analysis  calculates  the  joint  angle  velocities  given  the 
nozzle  velocity  in  Cartesian  space.  From  the  kinematic 
velocity  analysis  the  following  equation  relating  the  joint 
velocities  to  cartesian  velocities  was  developed: 

•  0 

y  =  Jh  q 

The  joint  angle  velocities  are  easily  found  by: 

q  =  Jh  1  y 

5.4.3.  Joint  Accelerations.  The  inverse  kinematic 
acceleration  analysis  calculates  the  joint  angle 
accelerations  given  the  nozzle  acceleration  in  Cartesian 
space.  From  the  kinematic  acceleration  analysis  the 
following  equation  relating  the  joint  accelerations  to 
cartesian  accelerations  was  developed: 
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3.  Inverse  Kinematics  -  A  Geometric  Approach  (Continued) 


y  =  Jh  q  +  Jh  q 

The  joint  angle  acclerations  are  easily  found  by: 

•»  ,  •*  *  # 

q  =  Jh_i  (  y  -  Jh  q  ) 

5.5.  Dynamics 

5.5.1.  Introduction  and  Review.  The  following  introduction 
and  review  of  robot  arm  dynamics  was  taken  from  the  book 
titled  "Robotics:  Control,  Sensing,  Vision,  and 
Intelligence"  by  Fu,  Gonzalez,  and  Lee. 

"Robot  arm  dynamics  deals  with  the  mathematical  formulation 
of  the  equations  of  robot  arm  motion.  The  dynamic  equations 
of  motion  of  a  manipulator  are  a  set  of  mathematical 
equations  describing  the  dynamic  behavior  of  the 
manipulator.  Such  equations  are  useful  for  computer 
simulation  of  the  robot  arm  motion,  the  design  of  suitable 
control  equations  for  a  robot  arm,  and  the  evaluation  of  the 
kinematic  design  and  structure  of  a  robot  arm.  In  this 
section  we  shall  concentrate  on  the  formulation, 
characteristics,  and  properties  of  the  dynamic  equations  of 
motion  suitable  for  control  purposes.  The  purpose  of 
manipulator  control  is  to  maintain  the  dynamic  response  of  a 
computer  based  manipulator  in  accordance  with  some 
prespecified  system  performance  and  desired  goals.  In 
general,  the  dynamic  responce  of  a  manipulator  directly 
depends  on  the  efficiency  of  the  control  algorithms  and  the 
the  dynamic  model  of  the  manipulator.  The  control  problem 
consists  of  obtaining  dynamic  models  of  the  physical  robot 
arm  system  and  then  specifing  corresponding  control  laws  or 
strategies  to  achieve  the  desired  system  response  and 
performance.  This  section  deals  mainly  with  the  former  part 
of  the  manipulator  control  problem;  that  is  modeling  and 
evaluating  the  dynamical  properties  and  behavior  of  computer 
controlled  robots. 

The  actual  dynamic  model  of  a  robot  arm  can  be  obtained  from 
known  physical  laws  such  as  the  laws  of  newtonian  mechanics 
and  lagrangian  mechanics.  This  leads  to  the  development  of 
the  dynamic  equations  of  motion  for  the  various  articulated 
joints  of  the  manipulator  in  terms  of  specified  geometric 
and  inertial  parameters  of  the  links.  Conventional 
approaches  like  the  Lagrange-Euler  (L-E)  and  Newton-Euler 
(N-E)  formulations  could  then  be  applied  systematically  to 
develop  the  actual  robot  arm  motion  equations.  Various 
forms  of  robot  arm  motion  equations  describing  the  rigid 
body  robot  arm  dynamics  are  obtained  from  these  two 
formulations,  such  as  Uicker's  Lagrange-Euler  equations 
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(Uicker  [1965],  Bejczy  [1974]),  Hollerbach's 
Recursive-Lagrange  (R-L)  equations  (Hollerbach  [1980]), 

Luh's  Newton-Euler  equations  (Luh  et  al.  [1980a]),  and  Lee's 
generalized  d'Alembert  (G-D)  equations  (Lee  et  al.  [1983]). 
These  motion  equations  are  "equivalent"  to  each  other  in  the 
Sense  that  they  describe  the  dynamic  behavior  of  the  same 
physical  robot  manipulator.  However  the  structure  of  these 
equations  may  differ  as  they  are  obtained  for  various 
reasons  and  purposes.  Some  are  obtained  to  achieve  fast 
computation  time  in  evaluating  the  nominal  joint  torques  in 
Servoing  a  manipulator,  other  are  obtained  to  facilitate 
control  analysis  and  synthesis,  and  still  others  are 
obtained  to  improve  computer  simulation  of  robot  motion. 

The  derivation  of  the  dynamic  model  of  a  manipulator  based 
on  the  L-E  formulation  is  simple  and  systematic.  Assuming 
rigid  body  motion,  the  resulting  equations  of  motion, 
excluding  the  dynamics  of  elctronic  (or  hydraulic)  control 
devices,  backlash,  and  gear  friction,  are  a  set  of 
second-order  coupled  nonlinear  differential  equations.  ... 
The  L-E  equations  of  motion  provide  explicit  state  equations 
for  robot  dynamics  and  can  be  utilized  to  analyze  and  design 
advanced  joint-variable  space  control  strategies.  To  a 
lesser  extent,  they  are  being  used  to  solve  for  the  forward 
dynamics  problem,  that  is,  given  the  desired  torques/forces, 
the  dynamic  equations  are  used  to  solve  for  the  joint 
accelerations  which  are  then  integrated  to  solve  for  the 
generalized  coordinates  and  their  velocities;  or  for  the 
inverse  dynamics  problem,  that  is,  given  the  desired 
generalized  coordinates  and  their  first  two  time 
derivatives,  the  generalized  forces/torques  are  computed. 

. . .  Unfortunately,  the  computaton  . . .  requires  a  fair  amount 
of  arithmetic  operations.  Thus,  the  L-E  equations  are  very 
difficult  to  utilize  for  real-time  control  purposes  unless 
they  are  simplified."10 

The  Lagrange  Euler  equation  is; 


where  L  =  Kinetic  Energy  -  Potential  Engergy  (  K  -  P  ) 

K  =  total  kinetic  energy 
P  =  total  potential  energy 
g  =  generalized  coordinates 

q  =  first  time  derivative  of  the  generalized 
coordinate 

~Qi=  generalized  force  or  torque  applied  to  the  system 
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"From  the  Lagrange  Euler  equation,  one  is  required  to 
properly  choose  a  set  of  generalized  coordinates  which 
completely  describe  the  location  (position  and  orienation) 
of  a  system  with  respect  to  a  reference  coordinate  frame. 
For  a  simple  manipulator  with  rotary-prismatic  joints, 
various  sets  of  generalized  coordinates  are  available  to 
describe  the  manipulator.  However,  since  the  angular 
positions  of  the  joints  are  readily  available  because  they 
can  be  measured  by  potentiometers  or  encoders  or  other 
sensing  devices,  they  provide  a  natural  correspondence  with 
the  generalized  coordinates.  ...  Thus,  in  the  case  of  a 
rotary  joint,  qi  =  0i,  the  joint  angle  span  of  the  joint; 
whereas  for  a  prismatic  joint,  q^^  =  dir  the  distance 
traveled  by  the  joint."1* 

5.5.2.  Potential  Energy.  The  potential  energy  of  link  "i" 
is  given  by: 


pi  = 


where:  m^  =  mass  of  link  "i" 
g  =  gravity 
hA  =  height  of  link  "i" 


Note  that  the  potential  energy  is  independent  of  joint 
velocity  and  is  only  dependent  on  joint  position  (q) .  The 
total  potential  energy  is  simply  the  sum  of  each  link's 
potential  energy. 


p  =  ipi 

I 


Since  the  potential  energy  is  independent  of  joint  velocity 
and  is  only  dependent  on  joint  position  (q)  the  Lagrange 
equation  can  by  rewritten  as: 


dt 


Tjl  +  I 


where :  rg.  = 

5.5.3.  Mass  Matrix.  The  mass  matrix  for  link  "i"  is 
represented  as: 


Mi 


I  mi  0  0  | 

I  0  mi  0  | 

I  0  0  m±  | 

l_  _l 
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5.5.4.  Inertial  Tensor.  The  inertial  tensor,  consisting  of 
the  moments  of  inertia  and  products  of  inertia  for  link  "i", 
is  a  constant  if  expressed  in  a  fixed  body  frame,  but  time 
varing  if  expressed  in  some  other  coordinate  system.  The 
body  fixed  inertial  tensor  for  link  "i"  is  represented  as: 


I 

-I 

-I 


XX 

“xy 

±xz 

I 

-I 

yx 

Tyy 

iyZ 

zx 

1zy 

zz 

I 

I 

I 

I 


or  using  the  radius  of  gyration: 


!xy  /  m. 


the  inertia  tensor  can  be  rewritten  as: 


n 


m. 


1  k  2 

k  2 

k  2  | 

*  kXX2 

kxy2 

kXZ2  | 

k^2 

k^2 

k722 

j  zx 

zy 

zz  j 

Kinetic 

Energy . 

Chasle 

5.5.5.  Kinetic  Energy.  Chasle's  Theorem  states  "The  most 
general  displacement  of  a  rigid  body  is  equivalent  to  a 
translation  of  some  point  in  the  body  plus  a  rotation  about 
an  axis  through  that  point".12 

iFor  convience  the  point  of  translation  will  be  at  the  CG. 


The  kinetic  energy  of  a  link  is  composed  of  two  parts; 
kinetic  energy  due  to  translational  velocity  at  the  eg,  and 
kinetic  energy  due  to  a  rotational  velocity  about  an  axis 
through  the  eg.  Thus,  the  kinetic  energy  for  link  "i"  is: 


KEcg  =  1/2 


*  V„„  M  V . 

eg  eg 


1/2 


where : 


V  =  velocity  of  link 


eg 

W 

P eg 


w  P  w 

eg 

"i"  at  the  eg 


angular  velocity  of  link  "i" 

Moment  and  Product  of  Inertia  for  link 
"i"  at  the  CG 


Since  the  coordinate  system  attached  to  link  "i"  is  usually 
not  at  the  link's  eg,  it  is  necessary  to  rewrite  the  kinetic 
energy  to  express  the  kinetic  energy  using  velocities  about 
the  link's  coordinate  system.  First  define  a  vector  from 
the  body  fixed  coordinate  system  to  the  eg: 
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_  I  cx  I 

°iC  =  I  c  I 

I  cz  I 

l_.  _l 

or  in  matrix  form  as: 


C 


I  o 
I  c2 
I  -c. 


Using  Figure  5-4,  the  kinetic  energy  is  rewritten  using  the 
velocity  at  coordinate  system  "i",  which  is  fixed  to 
link  "i”.  The  angular  velocity  at  coordinate  system  "i"  is 
the  same  as  the  angular  velocity  at  the  CG  since  the  angular 
velocity  vector  is  a  "free"  vector.  The  resulting  kinetic 
energy  expression  is  given  below: 

KE.  =  1/2  *  m.vTVi  +  n^vTCiW.  +  1/2  * 


If  we  redefine  as: 


I  Vi  |  velocity  at  coordinate  system  "i" 

vi  =  !  I 

I  |  angular  velocity  of  link  "i" 


and  define  Ui  as: 


I 

U±  =  | 


mil3  miCi 


I  miC.  Q  | 
l_  I 


then  it  can  be  easily  verified  that  the  kinetic  energy  of 
link  "i"  is: 

KEi  =  1/2  *  ViU.Vi 

The  total  kinetic  energy  is  found  by  summing  the  individual 
kinetic  energy  of  each  link.  The  total  kinetic  energy  for  a 
system  with  "n"  links  is  given  by: 
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Figure  5-4.  Kinetic  Energy  at  Coordinate  System  "i" 
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KE  ='  1/2  V^V. 

Note  that  is  in  cartesian  coordinates,  however,  we  want 
to  use  generalized  coordinates.  We  can  convert  cartesian 
coordinates  to  generalized  coordinate  by  using  a  subjacobian 
matrix  as  follows; 


The  derivation  and  a  method  to  calculate  the  subjacobian 
matrix  J ^  is  given  in  Figure  5-5. 

Subsituting  the  above  into  the  expression  for  the  total 
kinetic  energy  and  combining  terms,  the  kinetic  energy  can 
easily  be  rewritten  as; 


KE  =  1/2  *  qTW  q 
where  W  jfoji 

The  Inertial  matrix  W  is  symetric  and  positive  definite. 

5.5.6.  Lagrange  Equation.  In  a  paper  by  Gu  and  Loh  titled 
"Dynamic  Model  for  Industrial  Robots  Based  on  a  Compact 
Lagrangian  Formulation"  the  following  dynamic  equation  was 
formulated  using  the  Lagrange  Equation  along  with  the 
Kinetic  Energy  and  Potential  Engergy  equations  previously 
derived:13 

W  q  +  (WT  -  1/2W)  q  =  T  +  Zg 
where : 


W 


qT  ^W/<kq1 
qT  dW/^qn 


For  link  1  of  the  robotic  refueler  arm,  the  mass  matrix,  the 
inertia  tensor,  the  subjacobian  matrix,  and  the  Wx 
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Figlire  5-5.  Derivation  and  Method  for  Calculating  the  Subjacobian  uutrix 
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Figure  5-5.  Derivation  and  Method  for  Calculating  the  Subjacobian  Matrix 
(Continued) 
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jjaatrix  are  shown  in  Figure  5-6.  For  link  2  the  mass  matrix, 
%he  inertia  tensor,  the  subjacobian  matrix,  and  the  W2 
matrix  are  shown  in  Figure  5-7.  For  link  3  the  mass  matrix, 
the  inertia  tensor,  the  subjacobian  matrix,  and  the  W3 
matrix  are  shown  in  Figure  5-8.  For  link  4  the  mass  matrix, 
4he  inertia  tensor,  the  subjacobian  matrix,  and  the  W4 
matrix  are  shown  in  Figure  5-9.  The  total  W  matrix  for  the 
robotic  arm  is: 

#  =  Wx  +  W2  +  W3  +  W4 

The  calculations  for  computing  the  subjacobian  matrix  and 
the  W£  matrix  of  each  link  is  given  in  Appendix  A. 

The  compact  Lagrangian  formulation  stated  above  provides  a 
method  for  developing  a  control  strategy  for  the  robotic 
arm. 

5.6.  State  Space  Control  Model 

5.6.1.  Nonlinear  State  Space  Representation  in  Joint 
Coordinates.  The  nonlinear  state  space  representation  is 
driven  by: 

x  =  f  (x)  +  B  (x)  *u 
y  =  h(x)  =  h(q) 

Inhere,  for  the  robotic  arm,  small  "x"  contains  the  state 
Variables  of  joint  positions  and  joint  velocities  as  shown 
below: 

Joint  Position 
Joint  Velocity 


x  = 


q 

q 


The  time  derivative  of  "x"  is: 


4  I  q  I  Joint  Velocity 

*  =  I  .  .  I 

|  q  |  Joint  Acceleration 


The  output  "y"  is  position  and  orientation  of  the  nozzle: 


P  I 

I 

O  | 
_l 


Position 

Orienatation 
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Figure  5-6.  Rotating  Base  (Link  1)  Subjacobian  and  Matrices 
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8.  Fore  Arm  (Link  3)  Subjacobian  and  Matrices  (Continued) 
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Figure  5-9.  Nozzle  (Link  4)  Subjacobian  and  Matrices 
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Nozzle  (Link  4)  Subjacobian  and  Matrices  (Continued) 


The  input  torque  "u"  includes  both  the  dynamic  torque  and 
the  gravitation  torque  g: 


u 


T 1  +  Tgl 

Ta  +  2g  4 


Torque  on  joint  1 


Torque  on  joint  4 


From  the  Lagrange  equation  the  nonlinear  state  space  model 
is : 


|  W'1  [  u 


•  • 

q  i 

i 

WT  -  1/2W  )q  ]  | 


X  =  I  ,  . T  •  I 

|  W"1  (  1/2W  -  W  )q  | 


5.6.2.  Linear  State  Space  Representation  in  Global 
Coordinates.  To  control  the  global  position  and  orientation 
of  the  nozzle  (Coordinate  System  4)  the  nonlinear  state 
space  representation  given  in  joint  coordinates  must  be 
transformed  into  a  new  set  of  state  variable  in  global  or 
base  coordinates.  The  new  state  variables  in  global 
coordinates,  designated  as  capital  "X",  is  defined  as: 


I  0  | 

+  I  I  u 

I  W_1  | 


|  y  |  Global  Position  of  Nozzle 

X  =  |  .  | 

|  y  |  Global  Velocity  of  Nozzle 


The  time  derivative  of  capital  "X"  is: 


#  |  y  |  Global  Velocity  of  Nozzle 

X  =  |  .  .  I 

|  y  |  Global  Acceleration  of  Nozzle 


The  new  output  vector,  designated  as  capital  Y,  contains  the 
global  position  and  orientation  of  the  nozzle: 
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Position 


•  • 

I  P  I 
Y  =  I  I 

|  o  I  Orienatation 


The  linear  equations  in  state  space  are: 

X  *  AX  +  BV 


Y  =  CX 


Where  the  new  input  V  =  y.  The  linear  state  space  equations 
are  equivalent  to: 


X  = 


•  •  • 
i  y  i 
i  ..  i 
i  y  i 
i_  _i 


i  y  i 
i  i 
l  v  i 


i  o  i  i 
i  i 

10  0  1 


i  y  i 
1.1  + 
I  y  i 
t  i 


•  • 
i  o  i 
i  I  v 
I  i  i 
I  .1 


The  nonlinear  transformation  from  joint  coordinates  to 
global  or  base  coordinates  is  given  by  the  robot  arm 
kinematic  equations.  The  transformation  equations  are: 


y  =  h(q) 

y  =  Jh  q 

•  *  *  *  * 

y  =  Jh  q  +  Jh  q 

The  linear  input  vector  V  can  be  converted  into  joint 
torques  by  subsituting: 

q  =  Jh-1  (  y  -  Jh  q  )  =  Jh-1  (  V  -  Jh  q  ) 

into  the  Lagrange  equation.  The  torque  u  can  be  written  as 

B (X)  V  +  <*(x)  =  u 
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where 


<*(x)  =  (  WT-1/2W  )q  -  W  Jh_1  Jh  q 
B  (x)  =  W  Jh-1 

5.6.3.  Global  Proportional  Derivative  (PD)  Control  System. 

A  global  proportional  derivative  control  block  diagram  for 
the  robot  is  shown  in  Figure  5-10.  The  desired  global 
position  yd  is  compared  to  the  actual  position  y  to 
determine  the  position  error.  The  desired  global  velocity 
Yd  compared  to  the  actual  velocity  y  to  determine  the 
velocity  error.  The  input  vector  V  is  the  sum  of  the 
position  error  multiplied  by  a  gain  kl,  the  velocity  error 
multiplied  by  a  gain  k2,  and  the  desired  global 
acceleration  yd.  The  equation,  B(X)  V  +U(x),  can  then  be 
used  to  calculate  the  required  joint  torques  for  controling 
the  robot  system. 

5.6.4.  Performance.  Performance  and  stability  of  the 
control  system  depends  upon  the  values  selected  for  the 
pbsition  error  gain  kl  and  the  velocity  error  gain  k2. 
Performance  and  stability  is  determined  by  the  corresponding 
characteristic  equation.  The  characteristic  equation  for 
the  simple  linear  state  space  model  is  equivalent  to: 

det  [  A  I  -  (  A  -  B  K  )  ]  =  X2  +  k2*X  +  kl 

=  X2  +  2*f*wn*X  +  wn2 

The  gain  k2  is  equivalent  to  2***w  and  the  gain  kl  is 
equivalent  where  1  is  the  damping  ratio  and  w_  is  the 
natural  frequency.  For  critical  damping,  no  overshoot, 

*  ~ ^ •  The  settling  time  is  dependent  upon  both  the  damping 
ratio  and  the  natural  frequency. 

5.7.  Simulation  of_Glgbal__Pqsition  Controller 


5.7.1.  Computer  Program.  A  computer  program  was  written 
using  the  "C"  language  to  simulate  the  robot  refueler  arm's 
kinematic,  dynamic,  and  control  equations.  Appendix  C 
contains  all  the  source  code  for  the  simulation. 

The  source  code  was  compiled  using  the  "Optimizing  C86 
Compiler",  version  2.3,  by  Computer  Innovations,  Inc. 

Execution  of  the  program  begins  with  the  routine  called 
"main".  This  routine  calls  the  routine  "control".  By 
studing  the  "control"  routine,  the  reader  can  easily  follow 
program  exection. 
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Figure  5-10.  Global  Proportional  Derivative  (PD)  Control  Block  Diagram 


5.7.2.  Performance.  The  desired  robot  performance  was 
chosen  as  follows: 

•  0%  Overshoot  -  Critical  Damping  —  f  =  1.0 

•  Natural  Frequency  =  8.0  Hz. 

To  acheive  the  desired  performance,  gain  values  of  K1  =  64.0 
and  K2  =  16.0  were  chosen. 

5.7.3.  Simulation  Results.  The  desired  trajectory,  which 
the  contolled  robot  is  suppose  to  follow,  was  chosen  to  be 
72-inch  radius  circular  path  in  the  horizontal  plane  with  a 
simultaneous  48-inch  sinusoidal  rise  in  elevation  while 
maintaining  a  constant  approach  angle  of  90  degrees.  The 
equations  which  describe  the  desired  trajectory  path  are  as 
follows : 


Y  d  = 


72.0  *  cos  (  pi*t/10 . 0  ) 

72.0  *  sin  (  pi*t/10 . 0  ) 

-24.75  +  48.0  *  sin  (  pi*t/10.0  ) 
pi/2.0 


The  initial  conditions  in  the  global  X,  Y,  and  Z  direction 
were  purposely  offset  from  the  desired  trajectory  by  12.0 
inches  and  the  nozzle  orientation  was  offset  by  12  degrees 
to  evaluate  the  performance  of  the  control  system  against 
disturbances. 

The  results  of  the  simulation  are  given  in  Figure  5-11 
through  Figure  5-16.  Figure  5-11  shows  the  nozzle  global  X 
position  as  a  function  of  time  while  figure  5-12  shows  the 
nozzle  global  Y  position  and  Figure  5-13  shows  the  global  Z 
position  as  a  function  of  time.  Figure  5-14  shows  the 
nozzle  approach  angle  as  a  function  of  time.  In  the  four 
figures  the  desired  trajectory  is  illustrated  by  the  solid 
curve  and  the  actual  position  of  the  nozzle  is  illustrated 
by  the  dotted  curve. 

Figure  5-15  shows  the  nozzle  position  error  in  each 
direction  and  Figure  5-16  shows  the  nozzle  orientation 
error.  Initially  the  position  error  is  12.0  inches  and  the 
orientation  error  is  12.0  degrees  as  a  result  of  the  initial 
conditions.  After  one  second  the  position  error  is  nearly 
zero  and  the  nozzle  follows  the  desired  tractory  as 
expected. 
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Time  (sec) 
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APPENDIX  B 

MASS  AND  INERTIA  APPROXIMATIONS 


B-l 


B-2 


UJEI6HT  OF  EFCU  LINK 


ACTUAL  VAJEI6HT5  «RE  UIVKWOIUM. 

fTl HK)U F ft CTL’E  R.  KfcPOR TED  THAT  THE  TUROUE  ftROt/T  THE  SHOULDER 
-SDJ/V)T  IS  EtfUIVHlEWT  TO*  7? 0  Its  AT  /) 'OISTOA/CE  OF  <70"  WHEW  THE 
HRIM  IS  Fl/LLV  EXTEIUDED  AMD  FILLED  WITH  FUEL. 


(iSE.TS)  +  (A>  (jOfi)  -f  lx)  (?t)  -  (730YcID) 

ISE.7S  00  T  2IC  U)  +  72  UJ  -  2.(?*dfctC?) 

HHH  i  7S  U1  -  13  I  HDO 

IH  =  2.1  S  IbS 

KlfHJWD  UP  W  S  15<70  IbS 


UUEIE.HT  -  ~SOO  IbS 
WEI6HT  -  ZOO  IbS 
WEI6HT"  -  ISO  IbS 


AFT  ORO'I 
PURE  ARM 
KIOLLLE 


WElfcHT  -  ~  IbS 

rrm ss  =•  212. 78  /  20^,4  •«  o.ss 

m 

Xy  S  -jy^SB^.SO1*  S.7S1)  -  S.6S  lb S-SEd-IM 


CIRCULAR  DISK*  I 
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VOLUME-  trr1!-.  -  rr  0'5.75X5,.5fci  -  IS20-S3  IN3 

WEI6HT  ff  (l520.S3)(V).?B35)=  73/1.76  IbS  ^ 

WflSS  =  737. 76/?8f.‘/  =  I.  MS  -bS~SK 
'  /a; 

Xv  -  -Lrtir^c.  J_  (»,IIS)(,?  ,7SV?  -  ,0<S,V  lbS-<TFdJ-/A7 


CIKCULBR  DISK  2 

voli/me  -  7r  r*h  -  rr(7,sf(j?b7)  -  \DHb.n  no3 
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TQ-mL  iv=s.6s  + 


l/OS.V  +  3V.6  I 


LINK  Z 
LINK  2 

ft  FT  flRm 
RJRE  flRm 


LIRdULftR  CYZJfVDeR  7,7S  DIO. 


fIRCULftR  CYUA)DER 

U/EILH  T  ~  200  IbS 
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LIWK4 
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’7rt 
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voLurnE  =  it  r*h  r  t r  (z,eisf(i£)  -  bt=i  ,w3 — ►  -1  ttttol  volume 
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APPENDIX  C 

GLOBAL  "PD"  CONTROLLER  SIMULATION  PROGRAM 


C-l 


C-2 


CONTROL . C 
DEF_AB . C 
DEF_JH . C 
DEF_JHD . C 
DEF_RHS . C 
DEF_W .  C 
DEF_W1 . C 
DEF_W2 . C 
DEF_W3 . C 
DEF_W4 . C 
DEF_WT I L . C 
DIFFEQ.C 
INV_4X4M.C 
INV_KIN.C 
KIN.C 
MAIN . C 
MATRIX. C 
OUTPUT. C 
PLOT1.C 
PLOT2.C 
RK4_STEP.C 
TRAJ.C 


/*  control:  Global  Controller  for  robot  arms  */ 


/* - - */ 

/*  */ 

/*  Written  By:  James  A.  Aardema  */ 

/*  */ 

/*  Date:  November  28,1988  */ 

/*  */ 

/*  Modifications:  */ 

/*  */ 

/*  Called  by:  Main  */ 

/*  */ 

/*  Language:  C  */ 

/*  */ 

/*  Compiler  Options:  None  */ 

/*  */ 

/*  Machine  Dependencies:  None  */ 

/*  */ 

/*  Error:  None  */ 

/*  */ 

/*  Purpose:  Implement  the  global  position  and  velocity  controller  */ 

/*  */ 

/* - */ 

/* - Header  and  Include  Files - */ 

/*  */ 

/* - Symbolic  Constants - */ 

/*  */ 

/* - */ 


control  (  m,  h,  t,  tend,  kl,  k2,  q,  qd  ) 
double  h,  t,  tend,  kl,  k2,  q[4],  qd[4]; 
int  m; 

\ 

/* - Passed  Variables - */ 


/*  */ 

/*  m . Number  of  equations  to  be  integrated  */ 

t*  h . Integration  Step  Size  */ 

/*  t . Simulation  time  */ 

/*  tend . End  of  Simulation  Time  */ 

/*  kl . Position  Feedback  gain  */ 

/*  k2 . Velocity  Feedback  gain  */ 

/*  q . Joint  Positions  */ 

/*  qd . Joint  Velocity  */ 

/*  */ 

/* - */ 


double  y_d [ 4 ] ,  yd_d[4],  ydd_d[4]; 
double  qdd[4]; 
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double  y [4] ,  yd [4] ; 
double  pos_error [4 ] ,  vel_error [4] ; 
double  Alpha [4],  Beta  [4]  [4],  v[4]; 
double  X[8],  work [50]; 

/* - Local  Variables - */ 


/*  */ 

/*  y_d . Desired  global  position  */ 

/*  yd_d . Desired  global  velcity  */ 

/*  ydd_d . Desired  global  acceleration  */ 

/*  qdd. . . Joint  Accelerations  */ 

/*  y . Actual  Position  */ 

/*  yd..... . Actual  Veloctiy  */ 

/*  pos_error. . .Position  Error  -  (  Desired  -  Actual  )  */ 

/*  vel_error .. .Velocity  Error  -  (  Desired  -  Actual  )  */ 

/*  Alpha . Control  Matrix  */ 

/*  Beta . Control  Matrix  */ 

/*  v . Control  Input  */ 

/*  X . Vector  of  State  Equations  to  be  Integrated;  */ 

/*  Initial  Conditions  on  entry  -  Final  Conditions  on  return  */ 

/*  work . Working  Vector  for  the  Integration  routine  */ 

/*  */ 

/* - — - - */ 


extern  double  u[4]; 

/* - External  Variables - 

/* 

/*  u . Input  Torques 

/* 

/* - 

/* - Begin  Control  Loop  */ 

while  (  t  <=  tend  ) 

{ 


*/ 

*/ 

*/ 

*/ 

*/ 


/* - Get  desired  and  actual  positions,  velocities  and  accelerations  */ 

trajectory  (  t,  y_d,  yd_d,  ydd_d  );  /*  Get  desired  position  */ 

kinematics  (  q,  qd,  y,  yd  );  /*  Get  actual  pos  and  vel  */ 

/★---Calculate  position  and  velocity  error;  (  desired  -  actual  )  */ 

sub_matrix  (  y_d,  4,  1,  y,  pos_error  ); 
sub_matrix  (  yd_d,  4,  1,  yd,  vel_error  ); 

/* - Multiply  errors  by  proper  gains  */ 

smultjmatrix  (  pos_error,  4,  1,  pos_error,  kl  ); 

smult_matrix  (  vel_error,  4,  1,  vel  error,  k2  ); 
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/* - Calculate  control  input  */ 


add_matrix  (  pos_error,  4,  1,  vel_error,  v  ); 
add_matrix  (  v,  4,  1,  ydd_d,  v  ); 

/* - Define  Alpha  and  Beta  Matrices  */ 

def_Alpha_Beta  (  q,  qd,  Alpha,  Beta  ) ; 

/* - Caculate  input  Torques  */ 

mult_matrix  (  Beta,  4,  4,  v,  1,  u  ); 

add_matrix  (  Alpha,  4,  1,  u,  u  ); 

/* Call  routines  to  output  information  and  plotting  data  */ 

/*  But  first  devide  the  position  error  by  kl  and  */ 

/*  divide  the  velcity  error  by  k2  for  proper  reporting  */ 

smult_matrix  (  pos_error,  4,  1,  pos_error,  1.0/kl  ); 

smult  matrix  (  vel  error,  4,  1,  vel  error,  1.0/k2  ); 


/* 

output 

( 

t  f 

y_ 

_d, 

y> 

yd_ 

d. 

yd. 

q/ 

qd. 

pos_ 

error, 

vel 

_error, 

V, 

u 

)  ; 

*/ 

plotl 

( 

t  t 

y 

d, 

y. 

yd 

d. 

yd. 

q  / 

qd. 

pos_ 

error, 

vel 

error, 

V, 

u 

)  ; 

/* 

plot2 

( 

t  f 

y_ 

_d, 

y> 

yd 

"d, 

yd, 

q/ 

qd, 

pos] 

_error, 

vel_ 

_error , 

V, 

u 

) ; 

*/ 

/* - Set  up  initial  conditions  for  integrating  */ 


X [0]  =  q [ 0 ] ;  X [ 1]  =  q [ 1 ] ;  X[2]  =  q[2];  X[3]  =  q [ 3 ] ; 

X [4 ]  =  qd [ 0 ] ;  X[5]  =  qd [ 1 ] ;  X [ 6]  =  qd[2];  X [7 ]  =  qd [ 3 ] ; 

rk4_step  (  m,  X,  work,  &h,  &t  ) ; 

/* - Get  joint  positions  and  velocities  from  the  integration  routine  */ 


q  [  0  ]  =  X  t  o  ]  ; 

qd  [  o  ] 

=  X  [  4  ]  ; 

q  [  1  ]  =  x  E 1  ]  ; 

qd  [  l  ] 

=  X  [  5  ] ; 

q[2]  =  X  [  2  ]  ; 

qd  [  2  ] 

=  X  [  6] ; 

q  [3]  =  x  E  3  3 ; 

qd  1 3  ] 

=  X [7]  ; 

} 

} 
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/*  def_Alpha_Beta:  Defines  the  Alpha  and  Beta  Matrix  */ 


/* - . - - - - - - - - - */ 

/*  */ 

/*  Written  By:  James  A.  Aardema  */ 

/*  */ 

/*  Date:  November  26,  1988  */ 

/*  */ 

•/*  Modifications:  */ 

/*  */ 

/*  Called  by:  * / 

/*  */ 

/*  Language:  C  */ 

/*  */ 

/*  Compiler  Options:  None  */ 

/*  */ 

/*  Machine  Dependencies:  */ 

/*  */ 

/*  Error:  */ 

/*  */ 

/*  Purpose:  */ 

/*  */ 

/* - - */ 

/* - Header  and  Include  Files - */ 

/*  */ 

/* - Symbolic  Constants - */ 

/*  */ 

/* - */ 


def_Alpha_Beta  (  q,  qd,  Alpha,  Beta  ) 
double  q [4],  qd[4].  Alpha [4],  Beta[4][4]; 

{ 

/* - Passed  Variables - */ 


/*  */ 

/*  q . Joint  Positions  */ 

/*  qd.. . Joint  Velocities  */ 

/*  Alpha . Alpha  Matrix  */ 

/*  Beta . Beta  Matrix  */ 

/*  */ 

/* - */ 


double  W [ 4 ] [4] ,  Wtilde [4 ] [4 ] ,  Jh[4][4],  Jh_inv[4] [4] ,  Jhd[4][4]; 
double  Wtilde_tp[4] [4] ,  tempm[4][4],  tempvl[4],  tempv2[4]; 
int  error; 

/* - Local  Variables - - - */ 


/*  */ 
/*  W . ....Weight  matrix  W  */ 
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/*  Wtilde . W  tilde  matrix 

/*  jh . Jacobian  Matrix 

/*  Jh_inv . Inverse  of  the  Jacobian  Matrix 

/*  jhd . Derivative  of  the  Jacobian  Matrix 

/*  Wtilde_tp. .. Transpose  of  the  W  tilde  Matrix 

/*  tempm . Temporary  Matrix 

/*  tempvl . Temporary  Vector  1 

/*  tempv2 . Temporary  Vector  2 

/*  error . Error  Flag  for  the  Matrix  Inversion  Routine 

/* 

/* - 


/* - External  Variables - 

/* 

/* - 

def_w  (  W,  q  ) ; 

def_Wtilde  (  Wtilde,  q,  qd  )  ; 

def_Jh  (  q,  Jh  ) ; 

inv  4x4matrix  (  Jh,  Jh  inv,  error  )  ; 


def  Jhd  (  q,  qd,  Jhd  )  ; 


mult_ 

matrix 

( 

w,  4, 

4, 

Jh  inv, 

4, 

Beta  ) ; 

mult_ 

matrix 

( 

Beta, 

4, 

4,  Jhd, 

4, 

tempm  ) ; 

mult’ 

matrix 

( 

tempm, 

4, 

4,  qd, 

1, 

tempv2  ) ; 

transpose_matrix  (  Wtilde,  4,  4,  Wtilde_tp  ); 
smult_matrix  (  Wtilde,  4,  4,  tempm,  0.5  ); 
subjmatrix  (  Wtilde_tp,  4,  4,  tempm,  tempm  ); 
multjrtiatrix  (  tempm,  4,  4,  qd,  1,  tempvl  ); 
sub_matrix  (  tempvl,  4,  1,  tempv2,  Alpha  ); 

} 


*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

.*/ 
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/*  def_Jh:  Defines  the  Jacobian  Martix  */ 

/* - - - - - - - */ 

/*  */ 

/*  Written  By:  James  A.  Aardema  */ 

/*  */ 

/*  Date:  November  10,  1988  */ 

/*  */ 

/*  Modifications:  */ 

/*  */ 

/*  Called  by:  */ 

/*  */ 

/*  Language:  C  */ 

/*  */ 

/*  Compiler  Options:  None  */ 

/*  */ 

/*  Purpose:  */ 

/*  */ 

/* - - - */ 

def_Jh  (  q,  Jh  ) 
double  q  [  4  ] ,  Jh [ 4 ] [ 4 ] ; 

{ 

/* - Passed  Variables - */ 

/*  */ 

/*  q.  .  .  . . Joint  Positions  */ 

/*  Jh . Jacobian  Matrix  */ 

/*  */ 

/* - - */ 

double  cl,  si,  c2,  s2,  c23,  s23,  c234,  s234; 

/* - Local  Variables - */ 

/*  */ 

/*  cl,  si.... Cosine  and  Sine  of  angle  1  -  Waist  Angle  */ 

/*  c2,  s2 . . . . Cosine  and  Sine  of  angle  2  -  Shoulder  Angle  */ 

/*  c23,  s23... Cosine  and  Sine  of  (  angle  2  +  angle  3  )  */ 

/*  c234,  s234.. Cosine  and  Sine  of  (  angle  2  +  angle  3  +  angle  4  )  */ 

/*  */ 

/* - - - */ 

extern  double  sin(),  cos(); 

/* - External  Variables - */ 

/*  */ 

/*  sin() . Sine  of  an  angle  */ 

/*  cos  () . Cosine  of  an  angle  */ 

/*  */ 

/* - - */ 
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/*  Remember  that  arrays  start  at  zero  */ 


/*  q[0]  =  Waist  Angle  */ 
/*  q[lj  =  Shoulder  Angle  */ 
/*  q [ 2 ]  =  Elbow  Angle  */ 
/*  q[3]  =  Wrist  Angle  */ 


/*  Calculate  some  local  variables  */ 


cl  =  cos  (  q[0]  ) ; 
c2  =  cos  (  q [ 1 ]  ) ; 

c23  =  cos  (  q [ 1 ]  +  q [ 2 ]  ) ; 

c234  =  cos  (  q [ 1 ]  +  q[2]  +  q[3]  )  ; 

/*  The  first  row  */ 


si  =  sin  (  q [ 0 ]  ) ; 
s2  =  sin  (  q [ 1 ]  ) ; 

s23  =  sin  (  q [ 1 ]  +  q[2]  ) 

s234  =  sin  (  q [ 1 ]  +  q [ 2 ] 


Jh [ 0]  [0]  = 
Jh[0]  [1]  = 
Jh[0]  [2]  = 
Jh[0]  [3]  = 


-36 . 0*sl*c234  - 
-36 . 0*cl*s234  - 
-36 . 0*cl*s234  - 
-36 . 0*cl*s234; 


72 . 0*sl*c23  -  72 . 0*sl*c2; 
72 . 0*cl*s23  -  72.0*cl*s2; 
72.0*cl*s23; 


/*  The  second  row  */ 


Jhtl]  [0] 

Jh[l]  [1] 
Jh[l] [2] 
Jh [  1  ]  [3] 


36 . 0*cl *c234  + 
-36 . 0*sl*s234  - 
-36 . 0*sl*s234  - 
-36 . 0*sl*s234; 


72 . 0*cl*c23  +  72 . 0*cl*c2; 
72 . 0*sl*s23  -  72.0*sl*s2; 
72 . 0*sl*s23; 


/*  The  third  row  */ 


Jh  [2]  [0] 
Jh  [2]  [1] 
Jh  [2]  [2] 
Jh [2]  [3] 


0.0; 

-36 . 0*c234  -  72 . 0*c23  -  72.0*c2; 
-36 . 0*c234  -  72 . 0*c23; 
-36.0*c234; 


/*  The  fourth  row  */ 


Jh [3]  [0]  =  0.0; 
Jh [3]  [1]  =  1.0; 
Jh [3]  [2]  =  1.0; 
Jh [3]  [3]  =  1.0; 


} 


q  [  3  ]  ) ; 


c-io 


/*  def  Jhd:  Defines  the  time  derivative  of  the  Jacobian  Matrix 


*/ 

/* - */ 

/*  */ 

/*  Written  By:  James  A.  Aardema  */ 

/*  */ 

/*  Date:  November  10,  1988  */ 

/*  */ 

/*  Modifications:  */ 

/*  */ 

/*  Called  by:  */ 

/*  */ 

/*  Language:  C  */ 

/*  */ 

/*  Compiler  Options:  None  */ 

/*  */ 

/*  Purpose:  */ 

/*  */ 

/* - */ 

def_Jhd  (  q,  qd,  Jhd  ) 
double  q [ 4 ] ,  qd [ 4 ] ,  Jhd [ 4 ]  [ 4 ]  ; 

{ 

/* - Passed  Variables - */ 

/*  V 

/*  q . Joint  Positions  */ 

/*  qd . Joint  Velocities  */ 

/*  Jhd . Time  derivative  of  the  Jacobian  Matrix  */ 

/*  */ 

/* - - - */ 

double  cl,  si,  c2,  s2,  c23,  s23,  c234,  s234; 

/* - Local  Variables - */ 

/*  */ 

/*  cl,  si.... Cosine  and  Sine  of  angle  1  -  Waist  Angle  */ 

/*  c2,  s2.... Cosine  and  Sine  of  angle  2  -  Shoulder  Angle  */ 

/*  c23,  s23... Cosine  and  Sine  of  (  angle  2  +  angle  3  )  */ 

/*  c234,  s234.. Cosine  and  Sine  of  (  angle  2  +  angle  3  +  angle  4  )  */ 

/*  */ 

/* - */ 

extern  double  sin(),  cos(); 

/* - External  Variables - */ 

/*  */ 

/*  sin() . .Sine  of  an  angle  */ 

/*  cos() . Cosine  of  an  angle  */ 

/*  */ 
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/*- 

' 

/* 

Remember  that  arrays  start  at  zero 

*/ 

/* 

q [ 0 ]  =  Waist  Angle 

*/ 

/* 

q[l]  =  Shoulder  Angle 

*/ 

/* 

q[2]  =  Elbow  Angle 

*/ 

/* 

q[3]  =  Wrist  Angle 

*/ 

/* 

Calculate  some  local  variables  */ 

cl  =  cos  (  q [ 0 ]  );  si  =  sin  (  q[0]  ); 

c2  =  cos  (  q [ 1 ]  ) ;  s2  =  sin  (  q [ 1 ]  ); 

c23  =  cos  (  q[l]  +  q [2 ]  );  s23  =  sin  (  q[l]  +  q[2]  ); 

c234  =  cos  (  q[l]  +  q [ 2 ]  +  q [ 3 )  );  s234  =  sin  (  q[l]  +  q [ 2 ]  +  q[3]  ); 

/*  The  first  row  */ 

Jhd [  0  ] [ 0 ]  =  -  36.0*cl*c234*qd[0]  +  36 . 0*sl*s234 * (qd [ 1 ] +qd [2 ] +qd [ 3 ] ) 

-  72.0*cl*c23*qd[0]  +  72 . 0*sl *s23* (qd [ 1 ] +qd [2 ] ) 

-  72.0*cl*c2*qd[0]  +  72 . 0*sl *s2*qd [ 1 ] ; 

Jhd [ 0 ] [ 1 ]  =  36.0*sl*s234*qd[0]  -  36 . 0*cl *c234 * (qd [ 1 ] +qd [ 2 ] +qd [ 3 ] ) 

+  72.0*sl*s23*qd[0]  -  72 . 0*cl *c23* (qd [ 1 ] +qd (2 ] ) 

+  72 . 0*sl *s2  *qd [ 0 ]  -  72.0*cl*c2* qd [ 1 ) ; 

Jhd [ 0 ] [ 2 ]  =  36.0*sl*s234*qd[0]  -  36 . 0 *cl *c234 * (qd [ 1 ] +qd [2 ] +qd [ 3 ] ) 

+  72.0*sl*s23*qd[0]  -  72 . 0*cl *c23* (qd [ 1 ] +qd [2 ] ) ; 

Jhd[0] [3]  =  36.0*sl*s234*qd[0]  -  36 . 0*cl*c234* (qd [ 1 ] +qd [2 ] +qd [ 3] ) ; 

/*  The  second  row  */ 

Jhd[l] [0]  =  -  36.0*sl*c234*qd[0]  -  36 . 0*cl*s234 * (qd [ 1 ] +qd [2 ] +qd [ 3 ] ) 

-  72.0*sl*c23*qd[0)  -  72 . 0 *cl *s23* (qd [ 1 ] +qd [2 ] ) 

-  72 . 0*sl *c2*qd [ 0 ]  -  72 . 0*cl*s2*qd [ 1 ] ; 

Jhd[l] [1]  =  -  36.0*cl*s234*qd[0]  -  36 . 0*sl *c234 * (qd [ 1 ] +qd [ 2 ] +qd [ 3 ] ) 

-  72.0*cl*s23*qd(0]  -  72 . 0*sl*c23* (qd[l] +qd[2] ) 

-  72.0*cl*s2*qd[0]  -  72 . 0*sl*c2*qd [ 1 ) ; 

Jhd[l] [2]  =  -  36.0*cl*s234*qd[0]  -  36 . 0*sl*c234* (qd [ 1 ] +qd [2 ] +qd [ 3 ] ) 

-  72.0*cl*s23*qd[0]  -  72 . 0*sl *c23* (qd [ 1 ] +qd [2 ] ) ; 

Jhd[l] [3]  =  -  36.0*cl*s234*qd[0]  -  36 . 0*sl *c234 * (qd [ 1 ] +qd [ 2 ] +qd [ 3 ] ) ; 

/*  The  third  row  */ 

Jhd [2]  [0]  =  0.0; 
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Jhd [ 2 ] [1] 

36 . 0*s234* (qd [ 1 ] +qd [ 2 ] +qd [ 3 ] ) 

+ 

72 . 0*s23* (qd [ 1 ] +qd [ 2 ] ) 

+ 

72 . 0*s2*qd [ 1 ] ; 

Jhd [ 2 ] [2] 

= 

36 . 0*s234* ( qd [ 1 ] +qd [ 2 ] +qd ( 3 ] ) 

+ 

72 . 0*s23* (qd [ 1 ] +qd [2 ] ) ; 

Jhd [2] [3] 

= 

36 . 0*s234* (qd [ 1 ] +qd ( 2 ] +qd [ 3 ] ) ; 

/*  The  fourth 

row  */ 

Jhd [ 3 ] [0] 

= 

0.0; 

Jhd [ 3 ] [1] 

= 

0.0; 

Jhd [ 3 ] [2] 

= 

0.0; 

Jhd [ 3 ]  [3] 

= 

0.0; 

} 
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/*  def  rhs  Lagrange:  Defines  the  Right  Hand  Side  of  the  Lagrange  equation  */ 


/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 


Written  By:  James  A.  Aardema 
Date:  November  26,  1988 

Modifications : 

Called  by: 

Language:  C 

Compiler  Options:  None 
Machine  Dependencies: 

Error : 

Purpose : 


*'/ 

*/ 

V 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 


/* - Header  and  Include  Files - */ 

/*  */ 

/* - Symbolic  Constants - */ 

/*  */ 

/* - */ 


def_rhs_Lagrange  (  u,  q,  qd,  qdd  ) 
double  u[4],  q[4],  qd[4],  qdd [4]; 

{ 


/* - Passed 

Variables- 

- */ 

/* 

*/ 

/*  u . . 

. Input 

Torques 

*/ 

/*  q . 

Positions 

*/ 

/*  q . 

Velocities 

*/ 

/*  q . . 

Accelerations 

*/ 

/* 

*/ 

/* - 

- */ 

double  W[4] [4] ,  Wtilde [ 4 ] [4 ] ,  W_inv[4][4],  Wtilde_tp [4] [4] ; 
double  tempml [4] [4] ,  tempm2 [4] [4] ,  tempvl[4],  tempv2[4]; 
int  error; 

/* Local  Variables - */ 

/*  */ 

/*  W . Weight  Matrix  */ 
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/*  Wtilde . W  tilde  Matrix  */ 

/*  W_inv . Inverse  of  the  Weight  Matrix  */ 

/*  Wtilde_tp. . .Transpose  of  the  W  tilde  Matrix  */ 

/*  tempml . Tempoary  Matrix  1  */ 

/*  tempm2 . Tempoary  Matrix  2  */ 

/*  tempvl . Tempoary  Vector  1  */ 

/*  tempv2 . Tempoary  Vector  2  */ 

/*  error . Error  Code  for  the  Matrix  Inversion  Routine  */ 

/*  */ 

/* - */ 

/* - External  Variables - */ 

/*  -  */ 
/* - - */ 


def_ 

def~ 

_w  (  W,  q  )  ; 
Wtilde  (  Wtilde, 

q#  qd  ) ; 

/* 

/* 

Define 

Define 

the 

the 

W  matrix 

Wtilde  Matrix 

*/ 

*/ 

inv 

4x4matrix  (  W,  W 

inv,  error  ) ; 

/* 

Invert 

the 

W  matrix 

*/ 

mult_matrix  (  W_inv,  4,  4,  u,  1,  tempvl  ); 

transpose_matrix  (  wtilde,  4,  4,  Wtilde_tp  );  /*  Transpose  w  tilde  */ 

smult_matrix  (  Wtilde,  4,  4,  tempml,  0.5  ); 
sub_matrix  (  Wtilde_tp,  4,  4,  tempml,  tempml  ); 
mult_matrix  (  W_inv,  4,  4,  tempml,  4,  tempm2  ); 
mult_matrix  (  tempm2,  4,  4,  qd,  1,  tempv2  ); 

sub_matrix  (  tempvl,  4,  1,  tempv2,  qdd  ); 


} 
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/*  def_w:  Calculate  the  total  Weight  matrix  for  the  arm  */ 

/* - */ 

/*  */ 

/*  Written  By:  James  A.  Aardema  */ 

/*  *  / 

/*  Date:  November  23,  1988  */ 

/*  */ 

/*  Modifications:  */ 

/*  */ 

/*  Called  by:  */ 

/*  */ 

/*  Language:  C  */ 

/*  */ 

/*  Compiler  Options:  None  */ 

/*  */ 

/*  Purpose:  This  subroutine  calculate  the  Inertial  matrix  of  link  1  */ 

/*  The  total  Inertial  matrix  for  the  robot  is  the  sum  of  each  link's  */ 

/*  Inertial  matrix.  */ 

/*  W  =  W1  +  W2  +  W3  +  W4  */ 

/*  */ 

/* - */ 

/* - Header  and  Include  Files - */ 

/*  */ 

/* - Symbolic  Constants - */ 

/*  */ 

/* - */ 

def_w  (  w,  q  ) 
double  w[4] [4] ,  q[4] ; 

{ 

/* - Passed  Variables - */ 

/*  */ 

/*  w . Weight  Matrix  for  robot  arm  */ 

/*  q . Joint  Positions  */ 

/*  */ 

/* - */ 

double  wi [4]  [4] ; 

/* - Local  Variables - */ 

/*  */ 

/*  wi . Weight  Matrix  for  Link  "i"  */ 

/*  */ 

/* - */ 

/* - External  Variables - */ 

/*  */ 


C-16 


def_wl  (  w,  q  )  ; 
def  w2  (  wi,  q  ) ; 


add] 

jmatrix  ( 

w,  4, 

4, 

wi, 

w 

) ; 

def" 

]w3  (  wi, 

q  >; 

add] 

jmatrix  ( 

w,  4, 

4, 

wi, 

w 

) ; 

def] 

]w4  (  wi, 

q  ); 

add" 

"matrix  ( 

w,  4, 

4, 

wi, 

w 

) ; 

/* 

Define 

WI 

*/ 

/* 

Define 

W2 

*/ 

/* 

Add  WI 

+  W2 

=  W 

*/ 

/* 

Define 

W3 

*/ 

/* 

(WI  +  1 

W2)  + 

W3  = 

W 

*/ 

/* 

Define 

W4 

*/ 

/* 

(WI  +  i 

W2  + 

W3)  + 

W4 

=  W  */ 
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/*  def_wl:  Calculate  the  Weight  matrix  of  link  1 

- - 

/* 

/*  Written  By:  James  A.  Aardema 
/* 

/*  Date:  November  17,  1988 
/* 

/*  Modifications: 

/* 

/*  Called  by: 

/* 

/*  Language:  C 
/* 

/*  Compiler  Options:  None 

/*  ... 

/*  Purpose:  This  subroutine  calculate  the  Inertial  matrix  of  link  1 

/*  The  total  Inertial  matrix  for  the  robot  is  the  sum  of  each  link's 

/*  Inertial  matrix. 

/*  W  =  W1  +  W2  +  W3  +  W4 

/* 

- - 

def_wl  (  w,  q  ) 
double  w[4][4],  q[4]; 

{ 

/* - Passed  Variables - 

/* 

/*  w . Weight  Matrix  for  link  1 

/*  q . Joint  Positions 

/* 

/* - 

/* - Local  Variables - 

/* 

/* - 


*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 


*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 


extern  double  Cxi,  Cyl,  Czl,  ml,  Ixxl,  Iyyl,  Izzl,  Ixyl,  Ixzl,  Iyzl; 
/* - External  Variables - 


/* 

/*  Cxi . X  distance  from  CS  to  CG 

/*  Cyl . Y  distance  from  CS  to  CG 

/*  Czl . Z  distance  from  CS  to  CG 

/*  ml . Mass 

/*  Ixxl . Moment  of  Inertia 

/*  iyyl . 

/*  Izzl . 

/*  Ixyl . Product  of  Inertia 


V 

*/ 

*/ 

*/ 

*7 

*/ 

*/ 

*/ 

*/ 

*/ 
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/*  Ixzl . 

/*  Iyzl . 

/* 

/* - - - 

zero_matrix  (  w,  4,  4  ); 
w[0]  1 0 ]  =  iyyl; 

} 
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/*  def_w2:  Calculate  the  Weight  matrix  W2  of  link  2 

/* - 

/* 

/*  Written  By:  James  A.  Aardema 
/* 

/*  Date:  November  17,  1988 
/* 

/*  Modifications: 

/* 

/*  Called  by: 

/* 

/*  Language:  C 
/* 

/*  Compiler  Options:  None 
/* 

/*  Purpose:  This  subroutine  calculate  the  Inertial  matrix  of  link  2 
/*  The  total  Inertial  matrix  for  the  robot  is  the  sum  of  each  link's 
/*  Inertial  matrix. 

/*  W  =  W1  +  W2  +  W3  +  W4 

/* 

/* - 

/* - Header  and  Include  Files - 

/* 

/* - Symbolic  Constants - 

/* 

/* - 

def_w2  (  w,  q  ) 
double  w[4][4],  q ( 4 ]  ; 

{ 

/* - Passed  Variables - 

/* 

/*  w . Weight  Matrix 

/*  q . Joint  Positions 

/* 

/* - 


*/ 

*/ 

*/ 

*/ 

*1 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 


*/ 

*/ 

*/ 

*/ 

*/ 

*/ 


double  U[6] [6] ,  J[6][4],  Jt[4][6],  temp[4][6]; 
double  c2,  s2; 

/* - Local  Variables - */ 


/*  */ 

/*  U . Inertial  Matrix  */ 

/*  J . Sub  Jacobian  Matix  */ 

/*  Jt . Transpose  of  the  Sub  Jacobian  Matrix  */ 

/*  temp . Temporary  Matrix  */ 

/*  c2,  s2.... Cosine  and  Sine  of  angle  2  -  Shoulder  Angle  */ 
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/*  */ 

/* - -*/ 

extern  double  Cx2,  Cy2,  Cz2,  m2,  Ixx2,  Iyy2,  Izz2,  Ixy2,  Ixz2,  Iyz2; 
extern  double  sin(),  cos(); 

/* - External  Variables - - - - - */ 

/*  */ 

/*  Cx2 . .X  distance  from  CS  to  CG  */ 

/*  Cy2.... . Y  distance  from  CS  to  CG  */ 

/•*  Cz2 . Z  distance  from  CS  to  CG  */ 

/*  m2 ... . . .Mass  */ 

/*  Ixx2 . Moment  of  Inertia  */ 

/*  iyy2 . . .  */ 

/*  Izz2 .  */ 

/*  Ixy2 . Product  of  Inertia  */ 

/*  Ixz2 .  */ 

/*  Iyz2  . . . .  */ 

/*  sin() . Sine  of  an  angle  */ 

/*  cos() . .Cosine  of  an  angle  */ 

/*  */ 

/* - - 

/*  q[0]  =  Waist  Angle  */ 

/*  q[lj  =  Shoulder  Angle  */ 

/*  q[2]  =  Elbow  Angle  */ 

/*  q[3]  =  Wrist  Angle  */ 

/*  Calculate  some  local  variables  */ 

c2  =  cos  <  q [ 1 ]  ) ; 
s2  =  sin  (  q [ 1 ]  ) ; 

/*  Define  the  U  matrix  */ 

U[0][0]  =  m2;  U  [0]  [  1  ]  =  0.0;  U  [0]  [2  ]  =  0.0; 

UtlHO]  =  0.0;  U[l][l]  =  m2;  U[l][2]  =  0.0; 

U[2][0]  =  0.0;  U [2] [ 1 ]  =  0.0;  U [2] [2 ]  =  m2; 

U [3]  [0]  =  0.0;  U [3] [ 1 ]  =  -Cz2*m2;  U[3][2]  =  Cy2*m2; 

U [4] [0]  =  Cz2*m2;  U[4][l]  =  0.0;  U [4] [2]  =  -Cx2*m2; 

U  [5]  [  0  ]  =  -Cy2*m2;  U  [5]  1 1  ]  =  Cx2*m2;  U  [5]  [2]  =  0.0; 

* 

U[0][3]  =  0.0;  U[0][4]  =  Cz2*m2;  U [0] [5]  =  -Cy2*m2; 

U [ 1 ] [ 3 ]  =  -Cz2*m2;  U [ 1 ] [ 4 ]  =  0.0;  U [ 1 ] [5 ]  =  Cx2*m2; 

U [2] [3]  =  Cy2*m2 ;  U [2 ] [ 4 ]  =  -Cx2*m2;  U [2] [5]  =  0.0; 

U [3] [3]  =  Ixx2;  U[3][4]  =  Ixy2;  U[3][5]  =  Ixz2; 

U[4][3]  =  Ixy2;  U [ 4 ] [4 ]  =  Iyy2;  U [4] [5]  =  Iyz2; 

U[5][3]  =  Ixz2;  U [5] [4 ]  =  Iyz2;  U[5][5]  =  Izz2; 
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/*  Define  the  Sub  Jacobian  Matrix  for  Link  2  */ 


zero_matrix  (  J,  6,  4  )  ; 

J [2 ] [0]  =  72 . 0*c2; 

J[3] [0]  =  -s2; 

J [ 4 ] [0]  =  -c2; 

J[l] [1]  =  72.0; 

J [5] [1]  =  1.0; 

/*  Calculate  the  W  matrix  * 

transpose_matrix  (  J,  6, 
mult_matrix  (  Jt,  4,  6,  U 
mult_matrix  (  temp,  4,  6, 


} 


,  Jt  ); 

6,  temp  ) ; 
J,  4,  w  ) ; 
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/*  def_w3:  Calculate  the  Weight  matrix  W3  of  link  3  */ 

/* - - */ 

/*  */ 

/*  Written  By:  James  A.  Aardema  */ 

/*  */ 

/*  Date:  November  17,  1988  */ 

/*  */ 

/*  Modifications:  */ 

/*  */ 

/*  Called  by:  */ 

/*  */ 

/*  Language:  C  */ 

/*  */ 

/*  Compiler  Options:  None  */ 

/*  */ 

/*  Purpose:  This  subroutine  calculate  the  Inertial  matrix  of  link  3  */ 

/*  The  total  Inertial  matrix  for  the  robot  is  the  sum  of  each  link's  */ 

/*  Inertial  matrix.  */ 

/*  W  =  W1  +  W2  +  W3  +  W4  */ 

/*  */ 

/* - - */ 

/* - Header  and  Include  Files - */ 

/*  */ 

/* - Symbolic  Constants - */ 

/*  */ 

/* - */ 


def_w3  (  w,  q  ) 
double  w[4][4],  q[4]; 

{ 

/* - Passed  Variables - */ 


/*  */ 

/*  w . Weight  Matrix  */ 

/*  q . Joint  Positions  */ 

/*  */ 

/* - */ 

double  U [ 6] [6] ,  J[6][4],  Jt[4][6],  temp[4][6]; 
double  c2,  s2,  c3,  s3,  c23,  s23; 

/* - Local  Variables - */ 

/*  */ 

/*  U . Inertial  Matrix  */ 

/*  J . Sub  Jacobian  Matix  */ 

/*  Jt . Transpose  of  the  sub  Jacobian  Matrix  */ 

/*  temp . Temporary  Matrix  */ 

/*  c2,  s2 . . . . Cosine  and  Sine  of  angle  2  -  Shoulder  Angle  */ 
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/*  c3,  s3.... Cosine  and  Sine  of  angle  3  -  Elbow  Angle  */ 
/*  c23,  s23... Cosine  and  Sine  of  (  angle  2  +  angle  3  )  */ 
/*  */ 
/* - */ 


extern  double  Cx3,  Cy3,  Cz3,  m3,  Ixx3,  Iyy3,  Izz3,  Ixy3,  Ixz3,  Iyz3; 
extern  double  sin(),  cos(); 


/* - External  Variables - */ 


/*  */ 

/*  Cx3 . X  distance  from  CS  to  CG  */ 

/*  Cy3 . Y  distance  from  CS  to  CG  */ 

/*  Cz3 . Z  distance  from  CS  to  CG  */ 

/*  m3 . Mass  */ 

/*  Ixx3 . Moment  of  Inertia  */ 

/*  Iyy3 .  */ 

/*  Izz3 .  */ 

/*  Ixy3 . Product  of  Inertia  */ 

/*  Ixz3 . .  */ 

/*  Iyz3 .  */ 

/*  sin() . Sine  of  an  angle  */ 

/*  cos  () . Cosine  of  an  angle  */ 

/*  */ 

/* - */ 


/*  q[0]  =  Waist  Angle  */ 
/*  q[lj  =  Shoulder  Angle  */ 
/*  q[2]  =  Elbow  Angle  */ 
/*  q[3]  =  Wrist  Angle  */ 


/*  Calculate  some  local  variables  */ 

c2  =  cos  (  q[l]  ) ; 
s2  =  sin  (  q  [  1 ]  ) ; 
c3  =  cos  (  q  [2]  ) ; 
s3  =  sin  (  q[2]  ) ; 

c23  =  COS  (  q [1 ]  +  q [2 ]  ); 
s23  =  sin  (  q [ 1 ]  +  q [ 2 ]  ) ; 

/*  Define  the  U  matrix  */ 


U[0]  [0]  = 

m3; 

U[0] [1]  = 

0.0; 

u [0] [2] 

=  0.0; 

U[l]  [0]  = 

0.0; 

U [11 [1]  = 

m3; 

U [1] [2] 

=  0.0; 

U[2]  [0]  = 

0.0; 

u [2] [1]  = 

0.0; 

U[2] [2] 

=  m3; 

u [3] [0]  = 

0.0; 

U[3] [1]  = 

-Cz3*m3; 

U [3] [2] 

=  Cy3*m3; 

U [ 4 ] [0]  = 

Cz3*m3; 

U[4] [1]  = 

0.0; 

U[4] [2] 

=  -Cx3*m3; 

U[5] [0]  = 

-Cy3*m3; 

U [5] [1]  = 

Cx3*m3; 

U[5] [2] 

=  0.0; 

U[0] [3]  = 

0.0; 

U[0] [4]  = 

Cz3*m3; 

u [0] [5] 

=  -Cy3*m3; 
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U[l] [3]  = 

-Cz3*m3; 

U[l]  [4]  = 

0.0; 

U[l]  [5]  = 

Cx3*m3; 

U[2] [3]  = 

Cy3*m3; 

U[2]  [4]  = 

-Cx3*m3; 

U[2]  [5]  = 

0.0; 

U[3] [3]  = 

Ixx3; 

U[3]  1 4 ]  = 

Ixy3; 

u [3] [5]  = 

Ixz3; 

U[4] [3]  = 

Ixy3; 

U[4)  [4]  = 

Iyy3; 

U [4] [5]  = 

Iyz3; 

U[5] [3]  = 

Ixz3; 

U[5] [4]  = 

Iyz3; 

U[5] [5]  = 

I  z  z  3 ; 

/*  Define  the  Sub  Jacobian  Matrix  for  Link  3  */ 


zero_matrix  (  J,  6,  4  ) ; 

J [2] [0]  =  72 . 0*c23  +  72 . 0*c2; 

J [3]  [0]  =  -s23; 

J [4]  [0]  =  -c23; 

J [0]  [1]  =  72 . 0*s3; 

J[l] [1]  =  72.0  +  72 . 0*c3; 

J [5]  [1]  =  1.0; 

J[l] [2]  =  72.0; 

J [5] [2]  =  1.0; 

/*  Calculate  the  W  matrix  */ 

transpose_matrix  (  J,  6,  4,  Jt  ); 
mult_matrix  (  Jt,  4,  6,  U,  6,  temp  ) ; 
mult__matrix  (  temp,  4,  6,  J,  4,  w  ); 
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/*  def  w4 :  Calculate  the  Weight  matrix  of  link  4 

- - 

/* 

/*  Written  By:  James  A.  Aardema 
/* 

/*  Date:  November  17,  1988 
/* 

/*  Modifications: 

/* 

/*  Called  by: 

/* 

/*  Language:  C 
/* 

/*  Compiler  Options:  None 

/  * 

/*  Purpose:  This  subroutine  calculate  the  Inertial  matrix  of  link^4 
/*  The  total  Inertial  matrix  for  the  robot  is  the  sum  of  each  link's 
/*  Inertial  matrix. 

/*  w  =  W1  +  W2  +  W3  +  W4 

/* 

- - 

/* - Header  and  Include  Files - 

/* 

/* - Symbolic  Constants - 

/* 

/* -  " 

def_w4  (  w,  q  ) 
double  w[4][4],  q[4]; 

{ 

/* - Passed  Variables - 


/* 

/*  w . Weight  Matrix 

/*  q . Joint  Positions 

/* 

/* - 


*/ 

—  */ 
*/ 
*/ 
*/ 
*/ 
*/ 
*/' 
*/ 
*/ 
*/' 
*/ 
*/ 
*/ 
*/ 
*/ 
*/ 
*/ 
*/ 
*/ 

__*/ 

--*/ 

*/ 

—  */ 
*/ 

__*/ 


--*/ 

*/ 

*/ 

*/ 

*/ 

__*/ 


double  U[6] [6] ,  J[6][4],  Jt [ 4 ] [ 6 ] ,  temp[4][6]; 

double  c2,  s2,  c3,  s3,  c23,  s23,  c4,  s4,  c34,  s34,  c234, 


s234 ; 


/* - Local  Variables 


/* 

/*  u . Inertial  Matrix 

/*  j . Sub  Jacobian  Matix 

/*  jt . Transpose  of  the  Sub  Jacobian  Matrix 

/*  temp . Temporary  Matrix 

/*  c2,  s2 . . . . Cosine  and  Sine  of  angle  2  -  Shoulder  Angle 


*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 
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/*  c3,  s3.... Cosine  and  Sine  of  angle  3  -  Elbow  Angle  */. 
/*  c4,  s4,... Cosine  and  Sine  of  angle  4  -  Wrist  Angle  */ 
/*  c23,  s23... Cosine  and  Sine  of  (  angle  2  +  angle  3  )  */ 
/*  c34,  s34... Cosine  and  Sine  of  (  angle  3  +  angle  4  )  */ 
/*  c234,  s234.. Cosine  and  Sine  of  (  angle  2  +  angle  3  +  angle  4  )  */ 
/*  */ 
/* - - - - - */ 


extern  double  Cx4,  Cy4,  Cz4,  m4,  Ixx4,  Iyy4,  Izz4,  Ixy4,  Ixz4,  Iyz4; 
extern  double  sin(),  cos(); 

/*- — External  Variables - - - - */ 


/*  */ 

/*  Cx4 . .X  distance  from  CS  to  CG  */ 

/*  Cy4 . Y  distance  from  CS  to  CG  */ 

/*  Cz4 . Z  distance  from  CS  to  CG  */ 

/*  m4 . Mass  */ 

/*  Ixx4 . Moment  of  Inertia  */ 

/*  Iyy4 ........  */ 

/ *  I z  z  4 . .......  */ 

/.*  Ixy4 . Product  of  Inertia  */ 

/*  Ixz4 .  */ 

/*  Iyz4 . .  .  */ 

/*  sin() . Sine  of  an  angle  */ 

/*  cos() . Cosine  of  an  angle  */ 

/*  */ 

/* - - - */ 

/*  q[0]  =  Waist  Angle  */ 

/*  q [ 1 ]  =  Shoulder  Angle  */ 

/*  q [2 ]  =  Elbow  Angle  */ 

/*  q[3]  =  Wrist  Angle  */ 


/*  Calculate  some  local  variables  */ 

c2  =  cos  (  q  [  1 ]  )  ; 

s2  =  sin  (  q  [  1  ]  )  ; 

c3  =  cos  (  q  1 2  ]  ); 
s3  =  sin  (  q[2]  )  ; 

c4  =  cos  (  q[3]  )  ; 

s4  =  sin  (  q [ 4 ]  ) ; 

c23  =  cos  (  q [ 1 ]  +  q [ 2 ]  )/ 

s23  =  sin  (  q[l]  +  q [ 2 ]  ); 

c34  =  cos  (  q[2]  +  q [ 3 ]  ); 

s34  =  sin  (  q [ 2 ]  +  q[3]  ) ; 

c234  =  cos  (  q [ 1 ]  +  q [2]  +  q [ 3 ]  ); 

s234  =  sin  (  q [ 1 ]  +  q [ 2 ]  +  q [ 3 ]  ); 
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/*  Define  the  U  matrix  */ 


U[0] [0] 

= 

m4  ; 

U [ 0]  [1] 

= 

0.0; 

U[0]  [2] 

= 

0.0; 

U[l] [0] 

= 

0.0; 

u [ 1 ) [1] 

= 

m4; 

U[l]  [2] 

0.0; 

U[2] [0] 

rr 

0.0; 

U[2]  [1] 

= 

0.0; 

U[2]  [2] 

= 

m4; 

u  [  3]  [0] 

= 

0.0; 

u [ 3] [1] 

= 

-Cz4  *m4 ; 

U[3]  [2] 

= 

Cy4  *m4 ; 

U[4] [0] 

= 

Cz4*m4; 

U[4] [1] 

= 

0.0; 

U[4] [2] 

= 

-Cx4*m4; 

U[5] [0] 

= 

-Cy4  *m4 ; 

U [5] [1] 

= 

Cx4  *m4 ; 

U[5] [2] 

— 

0.0; 

U[0] [3] 

= 

0.0; 

U[0] [4] 

= 

Cz4  *m4 ; 

U[0] [5] 

-Cy4  *m4 ; 

U[l] [3] 

= 

-Cz4*m4; 

U[l] [4] 

= 

0.0; 

U [ 1 ) [5] 

= 

Cx4*m4; 

U[2] [3] 

= 

Cy4*m4; 

U[2] [4] 

— 

-Cx4  *m4 ; 

U [2 ] [5] 

= 

0.0; 

U[3] [3] 

= 

Ixx4; 

U[3] [4] 

= 

Ixy4  ; 

U  [3]  [5] 

=: 

Ixz4 ; 

U[4] [3] 

= 

Ixy4; 

U [4] [4] 

= 

Iyy4; 

U[4] [5] 

= 

Iyz4; 

U[5] [3] 

= 

Ixz4; 

U[5] [4] 

“ 

I  y  z  4 ; 

U[5] [5] 

= 

I  z  z  4 ; 

/*  Define  the  Sub  Jacobian  Matrix  for  Link  3  */ 


zero  matrix  (  J,  6,  4  )  ; 


J [ 2 ] [ 0 ]  =  36 . 0*c234  +  72.0*c23  +  72.0*c2; 

J [ 3 ] [0]  =  -s234; 

J [ 4 ] [0]  =  -c234; 

J [ 0 ] [ 1 ]  =  72 . 0*s4  +  72.0*s34; 

J [ 1 ] [ 1 ]  =  36.0  +  72 . 0*c4  +  72.0*c34; 

J [ 5 ] [1]  =  1.0; 


J[0] [2]  =  72 . 0*s4 ; 

J[l] [2]  =  36.0  +  72 . 0*c4; 

J [5 ] [2]  =  1.0; 


J[l] [3]  =  36.0; 

J[5] [3]  =  1.0; 


/*  Calculate  the  W  matrix  W  =  J'  U  J  */ 


transpose_matrix  (  J,  6,  4,  Jt  ); 
mult_matrix  (  Jt,  4,  6,  U,  6,  temp  ); 
mult_matrix  (  temp,  4,  6,  J,  4,  w  ); 


} 


/*  def_Wtilde:  Defines  the  W  tilde  Weight  Matrix  */ 

/* - */ 

/*  */ 

/*  Written  By:  James  A.  Aardema  */ 

/*'  */ 

/*  Date:  November  28,  1988  */ 

/*  */ 

/*  Modifications:  */ 

/*  */ 

/*  Called  by:  */ 

/*  */ 

/*  Language:  C  */ 

/*  */ 

/*  Compiler  Options:  None  */ 

/*  */ 

/*  Purpose:  Define  the  W  tilde  matrix  which  is  used  in  the  */ 

/*  Lagrange  Equation  */ 

/*  */ 

/* - */ 


def_Wtilde  (  Wtilde,  q,  qd  ) 
double  Wtilde [4] [4] ,  q[4],  qd [ 4 ] ; 
{ 


/*- 

— Passed 

Variables - 

- */ 

/* 

*/ 

/* 

Wtilde . , 

. W  tilde  matrix 

*/ 

/* 

q.  .  - - 

. Joint  Positions 

*/ 

/* 

qd . 

. Joint  Velocities 

*/ 

/* 

*/ 

/*- 

— 

— 

- */ 

double  cl,  si,  c2,  s2, 
double  w2q211; 
double  w3q211; 
double  w3q311,  w3q322, 
double  w4q211; 
double  w4q311,  w4q322, 
double  w4q411,  w4q422, 
w4q442,  w4q443; 


c3,  s3. 


w3q323, 

w4q323, 

w4q423. 


c4 ,  s4,  c23,  s23,  c34,  s34,  c234, 
w3q332; 

w4q324,  w4q332,  w4q342; 
w4q424,  w4q432,  w4q433,  w4q434, 


s234 ; 


/* Local  Variables - */ 

/*  */ 
/*  cl,  si.... Cosine  and  Sine  of  angle  1  -  Waist  Angle  */ 

/*  c2,  s2.... Cosine  and  Sine  of  angle  2  -  Shoulder  Angle  */ 

/*  c3,  s3.... Cosine  and  Sine  of  angle  3  -  Elbow  Angle  */ 

/*  c4,  s4 . . . . Cosine  and  Sine  of  angle  4  -  Wrist  Angle  */ 

/*  c23,  s23... Cosine  and  Sine  of  (  angle  2  +  angle  3  )  */ 

/*  c34,  s34... Cosine  and  Sine  of  (  angle  3  +  angle  4  )  */ 
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/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 


c234,  s234.. Cosine  and  Sine  of 
w2q21 1 . Partial  derivative 


w3q211 . Partial  derivative 

w3q311 . Partial  derivative 

w3q322 . Partial  derivative 

w3q323 . Partial  derivative 

w3q332  . Partial  derivative 

w4q21 1 . Partial  derivative 

w4q311 . Partial  derivative 

w4q322 . Partial  derivative 

w4q323 . Partial  derivative 

w4q324 . Partial  derivative 

w4q332 . Partial  derivative 

w4q342 . Partial  derivative 

w4q4 1 1 . Partial  derivative 

w4q422 . .Partial  derivative 

w4q423 . Partial  derivative 

w4q424  . Partial  derivative 

w4q4 32 . Partial  derivative 

w4q433 . Partial  derivative 

w4q434 . Partial  derivative 

w4q442 . Partial  derivative 

w4q443 . Partial  derivative 


(  angle  2  +  angle  3  +  angle  4  ) 


of 

W2 

wrt 

to 

q2 

- 

element 

(1,1) 

of 

W3 

wrt 

to 

q2 

- 

element 

(1,1) 

of 

W3 

wrt 

to 

q3 

- 

element 

(1,1) 

of 

W3 

wrt 

to 

q3 

- 

element 

(2,2) 

of 

W3 

wrt 

to 

q3 

- 

element 

(2,3) 

of 

W3 

wrt 

to 

q3 

— 

element 

(3,2) 

of 

W4 

wrt 

to 

q2 

- 

element 

(1,1) 

of 

W4 

wrt 

to 

q3 

- 

element 

(1,1) 

of 

W4 

wrt 

to 

q3 

- 

element 

(2,2) 

of 

W4 

wrt 

to 

q3 

- 

element 

(2,3) 

of 

W4 

wrt 

to 

q3 

- 

element 

(2,4) 

of 

W4 

wrt 

to 

q3 

- 

element 

(3,2) 

of 

W4 

wrt 

to 

q3 

— 

element 

(4,2) 

of 

W4 

wrt 

to 

q4 

- 

element 

(1,1) 

of 

W4 

wrt 

to 

q4 

- 

element 

(2,2) 

of 

W4 

wrt 

to 

q4 

- 

element 

(2,3) 

of 

W4 

wrt 

to 

q4 

- 

element 

(2,4) 

of 

W4 

wrt 

to 

q4 

- 

element 

(3,2) 

of 

W4 

wrt 

to 

q4 

- 

element 

(3,3) 

of 

W4 

wrt 

to 

q4 

- 

element 

(3,4) 

of 

W4 

wrt 

to 

q4 

- 

element 

(4,2) 

of 

W4 

wrt 

to 

q4 

- 

element 

(4,3) 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*r 

*/ 

*/ 

*7 

*/ 

*7 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

-*/ 


extern 

double 

Cxi , 

Cyl, 

Czl , 

ml , 

Ixxl , 

iyyi , 

Izzl , 

Ixyl, 

Ixzl , 

Iyzl  ; 

extern 

double 

Cx2, 

Cy2 , 

Cz2 , 

m2, 

Ixx2 , 

Iyy2, 

Izz2 , 

Ixy2, 

Ixz2, 

Iyz2; 

extern 

double 

Cx3, 

Cy3, 

Cz3 , 

m3 , 

Ixx3, 

iyy3, 

Izz3, 

Ixy3, 

Ixz3, 

I  y  z  3 ; 

extern 

double 

Cx4 , 

Cy4 , 

Cz4 , 

m4 , 

Ixx4 , 

Iyy4, 

Izz4 , 

Ixy4, 

Ixz4, 

Iyz4 ; 

extern  double  sin(),  cos  ()  ; 


/* - External  Variables 


/* 

/*  Cx . X  distance  from  CS  to  CG 

/*  Cy . Y  distance  from  CS  to  CG 

/*  Cz . Z  distance  from  CS  to  CG 

/*  m . Mass 

/*  ixx . Moment  of  Inertia 

/*  iyy . 

/*  Izz . 

/*  ixy . Product  of  Inertia 

/*  Ixz . 

/*  Iyz . 

/*  sin() . Sine  of  an  angle 


*/ 

*/ 

*/ 

*/ 

*7 

*/ 

*/ 

*-/ 

*/ 

*/ 

*/ 

*/ 

*/ 
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/*  cos() . Cosine  of  an  angle 

/* 

/* - — 


*/ 

*/ 

-*/ 


/* 

7* 

/* 

/* 

/* 


Remember  that  arrays  start  at  zero  */ 
q[0]  =  Waist  Angle  */ 
q[l]  =  Shoulder  Angle  * / 
q[2]  =  Elbow  Angle  */ 
q[3]  =  Wrist  Angle  */ 


/*  Calculate  some  local  variables  */ 


cl  =  cos  ( 

q[0]  ) 

/ 

si  =  sin  ( 

q [ 0]  )  ; 

c2  =  cos  ( 

q[l]  ) 

/ 

s2  =  sin  ( 

q  [  l  ]  ) ; 

c3  =  cos  ( 

q [2 ]  ) 

• 

f 

s3  =  sin  ( 

q  [  2  ]  )  ; 

c4  =  cos  ( 

q  [  3  ]  ) 

r 

s4  =  sin  ( 

q [3]  )  ; 

c23  =  cos 

(  q  [  1 1 

+  q  [2] 

) ; 

s23  =  sin 

(  q [ 1 ]  +  q [2]  )  ; 

c34  =  cos 

(  q[2] 

+  q  [  3] 

); 

s34  =  sin 

(  q [2]  +  q [3]  ); 

c234  =  cos 

(  q  [  i  ] 

+  q  [2] 

+  q  [  3  ] 

)  ;  s234  =  sin 

(  q [  1  ]  +  q [2 ]  + 

/* - Calculate  the 

partial 

derivatives  of  W2  wrt 

q2  */ 

w2q21 1  =  - 

2.0  * 

72.0 

★ 

72.0 

* 

c2  *  s2  *  m2 

- 

4.0  * 

72.0 

* 

c2 

* 

s2  *  Cx2  *  m2 

+ 

2.0  * 

Ixx2 

* 

s2 

* 

c2 

- 

2.0  * 

iyy2 

* 

c2 

* 

s2; 

/* - Calculate  the 

partial 

derivates  of  W3  wrt  q2  */ 

w3q211  =  - 

2.0  * 

72.0 

* 

72.0 

* 

c23  *  s23  *  m3 

- 

2.0  * 

72.0 

* 

72.0 

* 

s23  *  c2  *  m3 

- 

2.0  * 

72.0 

* 

72.0 

* 

c23  *  s2  *  m3 

- 

2.0  * 

72.0 

* 

72.0 

* 

c2  *  s2  *  m3 

- 

4.0  * 

72.0 

* 

c23 

* 

s23  *  Cx3  *  m3 

- 

2.0  * 

72.0 

* 

s23 

★ 

c2  *  Cx3  *  m3 

- 

2.0  * 

72.0 

★ 

c23 

★ 

s2  *  Cx3  *  m3 

+ 

2.0  * 

Ixx3 

* 

s23 

* 

c23 

- 

2.0  * 

iyy3 

* 

c23 

* 

s23; 

/* - Calculate  the 

partial 

derivates  of  W3  wrt  q3  */ 

w3q31 1  =  - 

2.0  * 

72.0 

* 

72.0 

* 

c23  *  s23  *  m3 

- 

2.0  * 

72.0 

* 

72.0 

* 

s23  *  c2  *  m3 

- 

4.0  * 

72.0 

* 

c23 

* 

s23  *  Cx3  *  m3 

- 

2.0  * 

72.0 

* 

s23 

* 

c2  *  Cx3  *  m3 

+ 

2.0  * 

Ixx3 

* 

s23 

★ 

c23 

- 

2.0  * 

iyy3 

* 

c23 

* 

s23; 

w3q322  =  - 

2.0  * 

72.0 

* 

72.0 

* 

s3  *  m3 
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-  2.0  *  72.0  *  s3  *  Cx3  *  m3; 

w3q323  =  -  72 . 0  *  72 . 0  *  s3  *  m3 

-  72.0  *  s3  *  Cx3  *  m3; 

w3q332  =  w3q323; 

/* - Caculate  the  partial  derivatives  of  W4  wrt  q2  */ 


w4q211  =  -  2.0  * 

36.0  *  36.0  * 

C234 

* 

s234 

A 

m4 

-  2.0  * 

36.0  *  72.0  * 

s23 

A 

c234 

A 

m4 

-  2.0  * 

36.0  *  72.0  * 

c23 

A 

s234 

A 

m4 

-  2.0  * 

36.0  *  72.0  * 

s2 

A 

c234 

A 

m4 

-  2.0  * 

36.0  *  72.0  * 

c2 

A 

s234 

A 

m4 

-  2.0  * 

72.0  *  72.0  * 

c23 

A 

s23 

A 

m4 

-  2.0  * 

72.0  *  72.0  * 

s2 

A 

c23 

A 

m4 

-  2.0  * 

72.0  *  72.0  * 

c2 

A 

s23 

A 

m4 

-  2.0  * 

72.0  *  72.0  * 

c2 

A 

s2 

A 

m4 

-  4.0  * 

36.0  *  c234  * 

s234 

A 

Cx4 

A 

m4 

-  2.0  * 

72.0  *  s23  * 

c234 

A 

Cx4 

A 

m4 

-  2.0  * 

72.0  *  c23  * 

s234 

A 

Cx4 

A 

m4 

-  2.0  * 

72.0  *  s2  * 

c234 

A 

Cx4 

A 

m4 

-  2.0  * 

72.0  *  c2  * 

s234 

A 

Cx4 

A 

m4 

+  2.0  * 

Ixx4  *  s234  * 

c234 

-  2.0  * 

iyy 4  *  c234  * 

s234; 

/* - Calculate  the 

partial  derivatives 

of  W4 

wrt 

w4q311  =  -  2.0  * 

36.0  *  36.0  * 

c234 

A 

s234 

A 

m4 

-  2.0  * 

36.0  *  72.0  * 

s23 

A 

c234 

A 

m4 

-  2.0  * 

36.0  *  72.0  * 

c23 

A 

s234 

A 

m4 

-  2.0  * 

36.0  *  72.0  * 

c2 

A 

s234 

A 

m4 

-  2.0  * 

72.0  *  72.0  * 

c23 

A 

s23 

A 

m4 

-  2.0  * 

72.0  *  72.0  * 

c2 

A 

s23 

A 

m4 

-  4.0  * 

36.0  *  c234  * 

s234 

A 

Cx4 

A 

m4 

-  2.0  * 

72.0  *  s23  * 

c234 

A 

Cx4 

A 

m4 

-  2.0  * 

72.0  *  c23  * 

s234 

A 

Cx4 

A 

m4 

-  2.0  * 

72.0  *  c2  * 

s234 

A 

Cx4 

A 

m4 

+  2.0  * 

Ixx4  *  s234  * 

c234 

-  2.0  * 

Iyy4  *  c234  * 

s  2  3  4  ; 

w4q322  =  +  2.0  * 

72.0  *  72.0  * 

s4  * 

c34  * 

m4 

-  2.0  * 

36.0  *  72.0  * 

s34  * 

m4 

-  2.0  * 

72.0  *  72.0  * 

s34  * 

c4  * 

m4 

-  2.0  * 

72.0  *  s34  * 

Cx4  * 

m4 ; 

w4q323  =  +  72.0  *  72.0  *  s4  *  c34  *  m4 

-  72.0  *  72.0  *  c4  *  s34  *  m4 

-  36.0  *  72.0  *  s34  *  m4 

-  72.0  *  s34  *  Cx4  *  m4 ; 
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w4q324  =  -  36.0  *  72.0  *  s34  *  m4 

-  72.0  *  s34  *  Gx 4  *  m4; 

w4q332  =  w4q323; 
w4q342  =  w4q324; 

/* - Calculate  the  partial  derivatives  of  W4  wrt  q4  */ 

w4q411  =  -  2.0  *  36.0  *  36.0  *  c234  *  s234  *  m4 

-  2.0  *  36.0  *  72.0  *  c23  *  s234  *  m4 

-  2.0  *  36.0  *  72.0  *  c2  *  s234  *  m4 

-  4.0  *  36.0  *  c234  *  s234  *  Cx4  *  m4 

-  2.0  *  72.0  *  c23  *  s234  *  Cx4  *  m4 

-  2.0  *  72.0  *  c2  *  s234  *  Cx4  *  m4 

+  2.0  *  Ixx4  *  s234  *  c234 

-  2.0  *  Iyy4  *  c234  *  s234 ; 

w4q422  =  2.0  *  72.0  *  72.0  *  c4  *  s34  *  m4 

+  2.0  *  72.0  *  72.0  *  s4  *  c34  *  m4 

-  2.0  *  36.0  *  72.0  *  s4  *  m4 

-  2.0  *  36.0  *  72.0  *  s34  *  m4 

-  2.0  *  72.0  *  72.0  *  s34  *  c4  *  m4 

-  2.0  *  72.0  *  72.0  *  c34  *  s4  *  m4 

-  2.0  *  72.0  *  s4  *  Cx4  *  m4 

-  2.0  *  72.0  *  s34  *  Cx4  *  m4; 

w4q423  =  72.0  *  72.0  *  c4  *  s34  *  m4 

+  72.0  *  72.0  *  s4  *  c34  *  m4 

-  72.0  *  72.0  *  s4  *  c34  *  m4 

-  72.0  *  72.0  *  c4  *  s34  *  m4 

-  2.0  *  36.0  *  72.0  *  s4  *  m4 

-  2.0  *  72.0  *  s4  *  Cx4  *  m4 

-  36.0  *  72.0  *  s34  *  m4 

-  72.0  *  s34  *  Cx4  *  m4; 

w4q424  =  -  36.0  *  72.0  *  s4  *  m4 

-  36.0  *  72.0  *  s34  *  m4 

-  72.0  *  s4  *  Cx4  *  m4 

-  72.0  *  s34  *  Cx4  *  m4; 

w4q432  =  w4q423; 

w4q433  =  -  2.0  *  36.0  *  72.0  *  s4  *  m4 

-  2.0  *  72.0  *  s4  *  Cx4  *  m4; 

w4q434  =  -  36.0  *  72.0  *  s4  *  m4 

-  72.0  *  S4  *  Cx4  *  m4; 

w4q442  =  w4q424; 
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w4q443  =  w4q434; 

/* - Define  the  W  tilde  Matrix  */ 

/*  Row  1  */ 

Wtilde [ 0 ] [0]  =  0.0; 

Wtilde [0] [1]  =  0.0; 

Wtilde [0] [2]  =  0.0; 

Wtilde [ 0 ] [3]  =  0.0; 

/*  Row  2  */ 

Wtilde [1] [0]  =  qd [ 0 ]  *  (  w2q211  +  w3q211  +  w4q211  ); 

Wtilde [1] [1]  =  0.0; 

Wtilde [ 1 ] [2]  =  0.0; 

Wtilde [ 1 ] [3]  =  0.0; 

/*  Row  3  */ 

Wtilde[2][0]  =  qd[0]  *  (  w3q311  +  w4q311  ); 

Wtilde [2] [1]  =  qd [ 1 ]  *  (  w3q322  +  w4q322  ) 

+  qd [ 2 ]  *  (  w3q332  +  w4q332  ) 

+  qd [ 3]  *  (  w4q342  )  ; 

Wtilde [2] [2]  =  qd[l]  *  (  w3q323  +  w4q323  ); 

Wtilde [2] [3]  =  qd[l]  *  (  w4q324  ); 

/*  Row  4  */ 

wtilde[3][0]  =  qd[0]  *  (  w4q411  ); 

Wtilde[3] [1]  =  qd [ 1 ]  *  w4q422  +  qd[2]  *  w4q432  +  qd [ 3 ]  *  w4q442; 

Wtilde[3] [2]  =  qd [ 1 ]  *  w4q423  +  qd[2]  *  w4q433  +  qd[3]  *  w4q443; 

Wtilde[3][3]  =  qd[l]  *  w4q424  +  qd[2]  *  w4q434; 
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/*  diffeq:  Sets  the  differential  equations  for  the  robot  arm  */ 

7* - -*/ 

/*  */ 

/*  Written  By:  James  A.  Aardema  */ 

/*  */ 

/*  Date:  November  28,  1988  */ 

/*  */ 

/*  Modifications:  */ 

/*  */ 

/*  Called  by:  rk4  step:  Integration  Routine  */ 

*  ■  /*  ~  */ 

/*  Language:  C  */ 

/*  */ 

/*  Compiler  Options:  None  */ 

/*  */ 

/*  Machine  Dependencies:  None  */ 

/*  */ 

/*  Error:  None  */ 

/*  */ 

/*  Purpose:  Sets  up  the  differential  equations  for  the  integration  */ 

/*  routine.  This  routine  call  def_rhs_Lagrange  to  get  the  right  hand  */ 

/*  side  of  the  Lagrange  differential  equations.  */ 

/*  */ 

/* - */ 

/* - Header  and  Include  Files - */ 

/*  */ 

/* - Symbolic  Constants - */ 

/*  */ 

/* - */ 

diffeq  (  f,  t,  y,  m  ) 
double  f [] ,  t,  y [ ] ; 
int  m; 

{ 

/* - Passed  Variables - */ 

/*  */ 

/*  f . First  derivative  of  the  state  equations  */ 

/*  t . Current  Time  */ 

/*  y . Current  Value  of  the  state  equations  */ 

/*  m . Number  of  differential  equations  to  be  integrated  */ 

/*  */ 

/* - */ 

double  q [ 4 ] ,  qd[4],  qdd[4J; 

/* - Local  Variables - */ 

/*  */ 
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/* 

q . . 

. Joint  Positions 

*/ 

/* 

qd . . 

. Joint  Velocities 

*/ 

/* 

qdd . 

. Joint  Accelerations 

*/ 

/* 

*/ 

/*- 

— 

- */ 

extern  double  u[4]; 

/* - External  Variables - */ 


/*  */ 

/*  u . Input  Torques  */ 

/*  */ 

/* - */ 

/*  Get  the  current  Joint  Positions  */ 

q [ 0 ]  =  y [ 0 ] ;  /*  y [ 0 ]  position  of  joint  1  */ 

q[l]  =  y[lj;  /*  y [ 1 ]  position  of  joint  2  */ 

q [2 ]  =  y [ 2 ] ;  /*  y [ 2 ]  position  of  joint  3  */ 

q[3]  =  y[3];  /*  y  [ 3 ]  position  of  joint  4  */ 

/*  Get  the  current  Joint  Velocities  */ 

qd[0]  =  y[4];  /*  y [ 4 ]  velocity  of  joint  1  */ 

qd[l]  =  y [ 5 ] ;  /*  y[5]  velocity  of  joint  2  */ 

qd[2]  =  y[6]/  /*  y  [  6 ]  velocity  of  joint  3  */ 

qd[3]  =  y[7];  /*  y [ 7 ]  velocity  of  joint  4  */ 


/*  Get  the  Joint  Accelerations  from  the  r.h.s  of  the  Lagrange  Equation  */ 
def_rhs_Lagrange  (  u,  q,  qd,  qdd  ) ; 

/* - Differential  Equations  to  be  integrated - */ 


/* - Integrate  Velocity  to  get  position  */ 


f [0]  =  qd [ 0 ] ; 
f[l]  =  qd [ 1 ] ; 
f [2]  =  qd [ 2 ] ; 
f  [3]  =  qd  [  3 ]  ; 


/*  Velocity  of  Joint  1  */ 
/*  Velocity  of  Joint  2  */ 
/*  Velocity  of  Joint  3  */ 
/*  Velocity  of  Joint  4  */ 


/* - Integrate  Acceleration  to  get  velocity  */ 


f  [  4 ]  =  qdd [ 0 ]  ; 

/* 

Accel 

of 

joint 

1 

*/ 

f [5]  =  qdd [ 1 ] ; 

/* 

Accel 

of 

Joint 

2 

*/ 

f [ 6 ]  =  qdd [ 2 ] ; 

/* 

Accel 

of 

Joint 

3 

*/ 

f [7 ]  =  qdd [3]; 

/* 

Accel 

of 

Joint 

4 

*/ 

} 
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/*  inv_4x4matrix:  Inverts  a  4x4  matrix  */ 

/* - - */ 

/*  */ 

/*  Written  By:  James  A.  Aardema  */ 

/*  */ 

/*  Date:  November  24,  1988  */ 

/*  */ 

/*  Modifications:  */ 

/*  */ 

/*  Called  by:  */ 

/*  */ 

/*  Language:  C  */ 

/*  */ 

/*  Error:  Returns  and  error  code  of  1  if  the  Determinant  is  zero  */ 

/*  and  set  the  inverse  matrix  to  all  zeros.  */ 

/*  */ 

/*  Compiler  Options:  None  */ 

/*  */ 

/*  Purpose:  This  subroutine  calculate  the  inverse  of  a  4x4  matrix  */ 

/*  using  a  brute  force  technique.  */ 

/*  */ 

/*  -1  adj  (  A  )  */ 

/*  A  =  -  */ 

/*  det  A  */ 

/* - - - */ 

/* - Header  and  Include  Files - */ 

/*  */ 

/* - Symbolic  Constants - */ 

/*  */ 

/* - - - */ 

inv_4x4matrix  (  a,  ai,  error  ) 
double  a[4] [4] ,  ai[4] [4] ; 
int  *error; 

{ 

/* - Passed  Variables - */ 

/*  */ 

/*  a . Matrix  A  */ 

/*  ai . Matrix  A  Inverse  */ 

/*  error . Error  Code  -  Set  to  1  if  determinant  is  zero;  0  otherwise  */ 

/*  */ 

/* - - */ 

double  det; 

register  int  i,  j; 

/* - Local  Variables - */ 
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Determinant  of  a  Matrix 

Index 

Index 


/* 

/* 

/* 

/* 

/* 

/* 


det 

i .  . 


*/ 

V 

*/ 

*/ 

*/ 

*/ 


double  extern  fabs  (); 

/* - External  Variables - 

/* 

/*  fabs . Floating  Point  Absolute  value 


/*  Calculate  the  Determinant  */ 


*error  =  0; 

det  =  a  [  0  ]  [  0  ] 

* 

( 

a  [1] [1] 

★ 

( 

a  [2]  [2 ]  *a  [ 3]  [3] 

-  a  [33  [23  *a  [23  [3] 

) 

- 

a  [2]  [1] 

★ 

( 

a[l] [2] *a [3] [3] 

-  a  [33 [2]*a[l]  [3] 

) 

+ 

a [3] [1] 

* 

( 

a  [  1  ]  [2] *a [2]  [3] 

-  a  [23  [23  *a  [1 3  [3] 

)  ) 

-  a  [  1  ]  [0] 

★ 

( 

a [0] [1] 

★ 

( 

a  [2]  [2]  *a  [3]  [3] 

-  a  [33  [2]  *a  [2]  [3] 

) 

- 

a [23  [1] 

★ 

( 

a  [  0 ]  [2]*a[3]  [3] 

-  a  [33  [23  *a  [03  [3] 

) 

+ 

a [3] [1] 

★ 

< 

a [03  [2] *a [2]  [3] 

-  a [23  [2]  *a [03  [3] 

)  ) 

+  a  [2]  [0] 

* 

( 

a [0] [1] 

* 

( 

a  [13  [23  *a  [33  [3] 

-  a  [33  [2]  *  a  [  1  ]  [3] 

) 

- 

a [ 1] [1] 

★ 

( 

a [03  [23  *a [33  [3] 

-  a  [33  [23  *a [ 03  [3] 

) 

+ 

a [3] [1] 

* 

( 

a  [ 0 ]  [23  *a  [13  [3] 

-  a  [13  [2]  *a  [0]  [3] 

)  ) 

-  a  [ 3]  [0] 

* 

( 

a  [0]  [1] 

★ 

( 

a  [1 3  [23  *a  [23  [3] 

-  a  [23  [23  *a  [13  [3] 

) 

- 

a [ 1 ] [1] 

★ 

( 

a [03  [23  *a [23  [3] 

-  a  [23  [23  *a  [03  [3] 

) 

+ 

a [2] [1] 

* 

( 

a [03  [23  *a  [13  [3] 

-  a  [13  [2]  *a  [03  [3] 

)  ); 

if  (  fabs  (  det 

) 

< 

1 

.  0E-008 

) 

printf  ("\nERROR  in  Matrix  Inversion  AlgorithmXn") ; 
printf  ("  No  unique  solution  exists\n”) ; 

printf  ("  Determinant  nearly  zero  (  less  than  1.0E-008  )\n"); 

printf  ("  Inverse  Matrix  will  be  set  to  all  zeros\n") ; 

*error  =  1; 

for  <  i  =  0;  i  <  4;  i++  ) 
for  (j=0;  j  <  4/  j++) 

ai [ i ] [ j ]  =  0.0;  /*  Zero  Inverse  Matrix  */ 

} 

goto  end; 

}  ~ 


/*  The  first  row  */ 
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ai [0]  [0]  = 

( 

a  tl] [1] 

* 

( 

a  [2]  [2]  *a  [3]  [3] 

- 

a  [2]  [3]  *a  [3]  [2] 

) 

-a [1 ] [2] 

* 

( 

a  [2]  [1]  *a  [3]  [3] 

- 

a  [2]  [  3  ]  *  a  [  3  ]  [1] 

) 

+a [1 ] [3] 

* 

( 

a  [2]  [1] *a [3]  [2] 

— 

a  t2]  [2]  *a  [3]  tl] 

) 

) 

/ 

det  ; 

ai[0]  tl]  =  - 

( 

a [0] [1] 

* 

( 

a [2]  [2] *a[3]  [3] 

- 

a  [2]  [3]  *a  [3]  [2] 

) 

-a [0] [2] 

* 

( 

a  [21  [1]  *a  [3]  [3] 

- 

a  [2]  [3]  *a  [3]  [1] 

) 

+a[0] (3] 

* 

( 

a  [2]  [1]  *a  [3]  [2] 

a  [2]  [2]  *a  [3]  [1] 

) 

) 

/ 

det; 

ai[0]  [2]  = 

( 

a [0 ] [1] 

* 

( 

a  [1]  [2]  *a  [3]  [3] 

- 

atl] [3]*at3] [2] 

) 

-a [0] [2] 

* 

( 

atl]  tl] *a[3] [3] 

- 

a  [1]  [3]  *a  [3]  [1] 

) 

+a [0] [3] 

* 

( 

a[l]  [l]*a[3] [2] 

— 

a  [  1  ]  [2]  *a  [3]  tl] 

) 

) 

/ 

det; 

ai [0] [3]  =  - 

( 

a [0] [1] 

* 

( 

a[l]  [2] *a  [2 ]  [3] 

— 

atl] [3] *a [2] [2] 

) 

-a  [0] [2] 

* 

( 

a  [  1  ]  [l]*at2]  [3] 

- 

a[l]  [3] *a [2] [1] 

) 

+a  [0] [3] 

* 

( 

a  [  1  ]  [1]  *a  [2]  [2] 

— 

a  [1]  [2] *a [2]  [1] 

) 

) 

/ 

det; 

*  The  second 

row  */ 

ai  [1]  [0]  =  - 

( 

atl] [0] 

★ 

( 

a  [2]  [2]  *a  [ 3]  [3] 

— 

a  [2]  [3]  *a  [3]  [2] 

) 

-a  [1] [2] 

* 

( 

a  [21  [0]  *a  [3]  [3] 

- 

a  [2]  [3]  *a  [3]  [0] 

) 

+a  [1] [3] 

* 

( 

a  [2]  [ 0]  *a  [ 3]  [2] 

— 

a  [2]  [2]  *a  [3]  [0] 

) 

) 

/ 

det; 

ai [1]  [1]  = 

( 

a [0] [0] 

* 

( 

a  [2]  [2]  *a  [3]  [3] 

- 

a [2]  [3]  *a  [3]  [2] 

) 

-a  [0] [2] 

* 

( 

a  [2]  [0] *a [3]  [3] 

- 

a  [2]  [3]  *a  [3]  [0] 

) 

+a  fO ]  [3] 

* 

( 

a [2]  [0]*a[3] [2] 

— 

a  [2]  [2]  *a  [3]  [0] 

) 

) 

/ 

det; 

ai  [1]  [2]  =  - 

( 

a  [0 ]  [0] 

* 

( 

a  [  1]  [2] *a [3]  [3] 

- 

a[l]  [3] *a  [3]  [2] 

) 

-a  [0 ]  [2] 

* 

( 

atl]  t0] *a [3] [3] 

- 

atl]  [3] *a [3]  [0] 

) 

+a  [0] [3] 

* 

( 

a[l]  [0] *a  [3]  1 2 ] 

— 

a  [1]  [2]  *a  [3]  [0] 

) 

) 

/ 

det ; 

ai [1] [3]  = 

( 

a [0] [0] 

* 

( 

a  [  1  ]  [2]  *a  [2]  [3] 

- 

a [13 [3] *a[2]  [2] 

) 

-a[0] [2] 

* 

( 

a  [  1  ]  [0] *a [2]  [3] 

a[l]  [33  *a [23  [0] 

) 

+a [0] [3] 

★ 

( 

a[l]  [0] *a [2]  [2] 

— 

atl]  [2]*a[2] [0] 

) 

) 

/ 

det  ; 

*  The  third  row  */ 

ai[2] [0]  = 

( 

atl]  [0] 

* 

( 

a  [2]  [  1  ]  *a  [3]  [3] 

— 

a  [2]  [3]  *a  [3]  [1] 

) 

-a[l] [1] 

★ 

( 

a  [2]  [0] *a [3]  [3] 

- 

a [2 ]  [33  *a [33  [0] 

) 

+a  [1]  [3] 

* 

( 

a  [2]  [0]  *a  [ 3]  [1] 

— 

a [23  [1] *a[3]  [0] 

) 

) 

/ 

det; 

ai [2]  [1]  =  - 

( 

a  [0]  [0] 

if 

( 

a [2]  [  1 ] *a  [ 3]  [3] 

- 

a [21  [3]*a[3]  [1] 

) 

-a[0] [1] 

if 

( 

a  t2]  [0] *a [3]  [3] 

- 

a  [2]  [3] *a [3]  [0] 

) 

+a[0] [3] 

it 

( 

a [2]  [0]  *a [3] [1] 

- 

a [2] [1] *a[3] [0] 

) 

) 

/ 

det  ; 

ai [2] [2]  = 

( 

a [0] [0] 

if 

( 

atl] [1] *a[3]  [3] 

- 

atl] [3]*a[3] [1] 

) 

-a [0]  [1] 

if 

( 

atl]  [0]  *a  [ 3] [3] 

- 

a [13 [3] *a[3]  [0] 

) 

+a  [0]  [3] 

if 

( 

a [  1  ]  [ 0] *a [3] [1] 

— 

a[l]  [13  *a [33  [0] 

) 

) 

/ 

det; 

ai  [2]  [3]  =  - 

( 

a [0] [0] 

if 

( 

a [ 1 ] [ 1 ] *a [2]  [3] 

- 

a [ 1  ]  [3] *a[2]  [1] 

) 

-a[0]  [1] 

★ 

( 

a  [  1  ]  [0] *a [2]  [3] 

- 

a[l] [33  *a [23 [0] 

) 
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/*  The  fourth 
ai [3] [0]  =  - 

ai [3] [1]  = 

ai  [3]  [2]  =  - 

ai [3] [3]  - 

end: 

r 

) 


+a [ 0] [3] 

* 

( 

all]  [0] *a [2] [1] 

row  */ 

( 

a[l] [0] 

* 

( 

a  [2]  [l]*a[3] [2] 

-all] 11] 

* 

( 

a [2]  [0] *a[3]  [2] 

+a[l] [2] 

★ 

( 

a [2 ] [0] *a[3] [1] 

( 

a  [ 0]  [0] 

* 

( 

a  [2 ]  [1]  *a  [3]  [2] 

-a [0]  11] 

★ 

( 

a  [2]  [0] *a [3]  [2] 

+a[0]  [2] 

* 

( 

a  [2 ]  [0]  *a  1 3]  [1] 

( 

a [0] [0] 

★ 

( 

all]  [1]  *a [3] [2] 

-a [0] 11] 

* 

( 

a[l]  [0]  *a [3]  12] 

+a[0] [2] 

★ 

( 

a[l]  [0] *a [3]  [1] 

( 

a [0] [0] 

* 

( 

all]  [1] *a [2] [2] 

-a [0]  11] 

* 

( 

all]  [0] *a[2]  [2] 

+a[0] [2] 

* 

( 

all]  10] *a[2]  [1] 

a [1] {1 ] *a  [2]  [0]  )  ) 


a[2] [2] *a[3]  [1]  ) 
a  [2  ]  [2] *a [3]  [0]  ) 
a [2 ]  [1] *a[3]  [0]  ) 

a  [2]  [2 ] *a [ 3 ]  [1]  ) 
a [2 ] [2] *a[3] [0]  ) 
a [2 ]  [1] *a[3]  [0]  ) 

a [ 1 ] [2] *a[3)  [1]  ) 
a [1]  [2] *a [3]  [0]  ) 
all] [ 1 ] *a [ 3]  [0]  ) 

all] [2] *a[2]  [1]  ) 
all]  [2] *a  [2]  [0]  ) 
all] [1] *a[2]  [0]  ) 


/  det; 

I  /  det; 

)  /  det ; 

)  /  det; 

)  /  det ; 
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/*  inv  kin:  Inverse  Kinematics 


*/ 


/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/*■ 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 


Written  By:  James  A.  Aardema 
Date:  November  26,  1988 

Modifications : 

Called  by: 

Language :  C 

Compiler  Options:  None 
Machine  Dependencies: 

Error : 

Purpose : 


•*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 


/* - Header  and  Include  Files - */ 

/*  */ 

/* - Symbolic  Constants - */ 


inv_kin  (  y,  yd,  q,  qd  ) 
double  y [4] ,  yd [4],  q[4],  qd[4]; 

{ 

/* - Passed  Variables - */ 


/*  y.... . Global  (Cartesian)  Position  of  nozzle  */ 

/*  yd.... . Global  (Cartesian)  Velocity  of  nozzle  */ 

/*  q . Joint  Positions  */ 

/*  qd . Joint  Velocities  */ 


double  Jh[4][4],  Jh_inv[4] [4] ; 
double  cl,  si,  c2,  s2,  ca,  sa; 
double  p3xg,  p3yg,  p3zg; 
double  p3xl ,  p3yl,  p3zl; 

double  r,  salpha,  calpha,  sbeta,  cbeta,  sgamma,  cgamma; 

/* - Local  Variables - */ 
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/*  *  / 

/*  Jh . Jacobian  Matrix  */ 

/*  Jh_inv . Inverse  of  the  Jacobian  Matrix  */ 

/*  clT . Cosine  of  Angle  1;  Waist  Joint  */ 

/*  si . Sine  of  Angle  1;  Waist  Joint  */ 

/*  c2 . Cosine  of  Angle  2;  Shoulder  Joint  */ 

/*  s2 . Sine  of  Angle  2;  Shoulder  Joint  */ 

/*  ca . Cosine  of  the  Approach  Angle  */ 

/*  sa . Sine  of  the  Approach  Angle  */ 

/*  p3xg . Global  X  distance  to  CS  3  */ 

/*  p3yg . Global  Y  distance  to  CS  3  */ 

/*  p3zg . Global  Z  distance  to  CS  3  */ 

/*  p3xl . X  distance  from  CS  1  to  CS  3  */ 

/*  p3yl . Y  distance  from  CS  1  to  CS  3  */ 

/*  p3zl . Z  distance  from  CS  1  to  CS  3  */ 

/*  r . Distance  from  CS  1  to  CS  3  in  the  XI, Y1  plane  */ 

/*  salpha . Sine  of  Alpha  */ 

/*  calpha . Cosine  of  Alpha  */ 

/*  sbeta . Sine  of  Beta  */ 

/*  cbeta . Cosine  of  Beta  */ 

/*  sgamma . Sine  of  Gamma  */ 

/*  cgamma . Cosine  of  Gamma  */ 

/*  */ 

/* - */ 

extern  double  atan2(),  cos(),  sin(),  sqrt(); 

/* - External  Variables - */ 

/*  */ 

/*  atan2 . Arctan  */ 

/*  sin . Sine  of  an  Angle  */ 

/*  cos . Cosine  of  an  Angle  */ 

/*  sqrt . Square  Root  */ 

/*  */ 

/* - */ 

/*  Remember  that  arrays  start  at  zero  */ 

/*  q [ 0 ]  =  Waist  Angle  */ 

/*  q 1 1 ]  =  Shoulder  Angle  */ 

/*  q[2]  =  Elbow  Angle  */ 

/*  q[3]  =  Wrist  Angle  */ 

/* - Define  some  local  variables  */ 

ca  =  cos  (  y [3]  ) ; 

sa  =  sin  (  y [3]  ) ; 

/* - Calculate  the  angle  of  the  rotating  base;  Joint  1;  q[0]  */ 

q [ 0 ]  -  atan2  (  y[l],  y  [  0]  ); 

cl  =  cos  (  q[0]  ) ; 
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si  =  sin  (  qtO]  ) ; 

/* - Calculate  the  global  position  of  Coordinate  System  3  */ 

p3xg  =  y[0]  -  36.0*cl*ca; 
p3yg  =  y[l]  -  36.0*sl*ca; 
p3zg  =  y [ 2 ]  +  36.0*sa; 

/* - Calculate  the  position  of  CS  3  with  respect  to  CS  1  */ 

p3xl  =  p3xg*cl  +  p3yg*sl; 
p3yl  =  -p3zg  +  11.25; 
p3zl  =  -p3xg*sl  +  p3yg*cl; 

/* - Calculate  the  distance  from  CS  1  to  CS3  */ 

r  =  sqrt  (  p3xl*p3xl  +  p3yl*p3yl  ) ; 

/* - Calculate  the  angle  of  the  shoulder;  Joint  2;  q[l]  */ 

salpha  =  p3yl  /  r; 
calpha  =  p3xl  /  r; 

cbeta  =  r  *  r  /  (  144.0  *  r  ); 
sbeta  =  sqrt  (  1.0  -  cbeta*cbeta  ); 

s2  =  salpha*cbeta  -  calpha*sbeta; 
c2  =  calpha*cbeta  +  salpha*sbeta; 

q[l]  =  atan2  (  s2,  c2  ); 

/* - Calculate  the  angle  of  the  elbow;  Joint  3;  q[2]  */ 

cgamma  =  (  -(r*r)  +  10368.0  )  /  10368.0; 
sgamma  =  sqrt  (  1.0  -  cgamma*cgamma  ); 

q[2]  =  3.141592654  -  atan2  (  sgamma,  cgamma  ); 

/* - Calculate  the  angle  of  the  wrist;  Joint4;  q [ 3 ]  */ 

q [ 3 ]  =  y [3]  -  q[2]  -  q [ 1 ] ; 

/* - Calculate  the  joint  velocities  */ 

def_Jh  (  q,  Jh  ) ; 
inv_4x4matrix  (  Jh,  Jh_inv  ) ; 

mult_matrix  (  Jh_inv,  4,  4,  yd,  1,  qd  ) ; 
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/*  kinematics:  Calculate  global  position  and  velocity  given  joint  angle  */ 
/*  and  joint  velocities 


/* - 

/* 

/*  Written  By:  James  A.  Aardema 
/* 

/*  Date:  November  10,  1988 
/* 

/*  Modifications: 

/* 

/*  Called  by: 

/* 

/*  Language:  C 
/* 

/*  Compiler  Options:  None 
/* 

/*  Purpose: 

/*  q 1 0 ]  =  Waist  Angle 
/*  q [ 1 ]  =  Shoulder  Angle 
/*  q [2 ]  =  Elbow  Angle 
/*  q[3]  =  Wrist  Angle 
/* 

/* - 


__*/ 

*/ 

*7 

*/ 

*/ 

*/ 

*/ 

*/ 

*; 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

— */ 


kinematics  (  q,  qd,  y,  yd  ) 
double  q[4],  qd[4],  y[4],  yd [4]; 
{ 

/* - Passed  Variables - 


/*  q . Joint  Positions 

/*  qd . Joint  Velocities 

/*  y . Global  Position  of  nozzle 

/*  yd . Global  Velocity  of  nozzle 


double  Jh[4] [4] ; 

double  cl,  si,  c2,  s2,  c23,  s23,  c234,  s234; 

/* - Local  Variables - 

/* 

/*  Jh . Jacobian  Matix 

/*  cl,  si.... Cosine  and  Sine  of  angle  1  -  Waist  Angle 

/*  c2,  s2.... Cosine  and  Sine  of  angle  2  -  Shoulder  Angle 

/*  c23 ,  s23... Cosine  and  Sine  of  (  angle  2  +  angle  3  ) 

/*  c234 ,  s234.. Cosine  and  Sine  of  (  angle  2  +  angle  3  +  angle  4  ) 

/* 

- - 


*7 

*/ 

*/ 

*V 

*/ 

*/ 

*/ 

*/ 

*/ 
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extern  double  sin  Or  cos(); 


/* - External  Variables - 

/* 

/*  sin  () . Sine  of  an  angle 

/*  cos() . Cosine  of  an  angle 

/* 

/*-- - 


■*/ 

*/ 

*/ 

V 

*/ 

-*/ 


/* 

/* 

/* 

/* 

/* 


Remember  that  arrays  start  at  zero 
q[0]  =  Waist  Angle 
q  [  1 ]  =  Shoulder  Angle 
q [ 2 ]  =  Elbow  Angle 
q[3]  =  Wrist  Angle 


*/ 

*/ 

*/ 

*/ 

V 


/*  Calculate  some  local  variables  */ 

cl  =  cos  <  q [ 0 ]  )  ; 
c2  =  cos  (  q  1 1 1  ) ; 


si  =  sin  (  q [ 0 ]  )  ; 
s2  =  sin  (  q [ 1 ]  ) ; 


c23  =  cos  (  q[l]  +  q[2]  );  s23  =  sin  (  q[l]  +  q[2]  ); 

c234  =  cos  (  q 1 1 ]  +  q [2]  +  q[3]  );  s234  =  sin  (  q[l]  +  q[2]  +  q[3]  ); 


/*  Calculate  the  global  position  of  the  nozzle  */ 


y  [0]  = 
y  [l]  = 

y  [2]  = 
y  [3]  = 


36 . 0*cl*c234  +  72 . 0*cl*c23 
36 . 0*sl*c234  +  72 . 0*sl*c23 
-36 . 0*s234  -  72 . 0*s23 

q [ 1 ]  +  q[2]  +  q [ 3 ] ; 


+  72 . 0*cl *c2 ; 

+  72 . 0*sl *c2 ; 

-  72 . 0*s2  +  11.25; 


/*  Define  the  Jacobian  Matrix  */ 
def__Jh  (  q,  Jh  )  ; 

/*  Calculate  the  nozzle  velocity  using  yd  =  Jh*qd  */ 
mult  matrix  (  Jh,  4,  4,  qd,  1,  yd  ); 


} 
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/*  main:  Main  Entry  upon  Start  of  Execution 


*/ 


/* - 

/* 

/*  Written  By:  James  A.  Aardema 
/* 

/*  Date:  November  29,  1988 
/* 

/*  Modifications: 

/* 

/*  Called  by: 

/* 

/*  Language:  C 
/* 

/*  Compiler  Options:  None 
/* 

/*  Machine  Dependencies:  None 
/* 

/*  Error:  None 
/* 

/*  Purpose:  Start  Execution  of  program,  Initialize  variables, 
/*  control  process 

/* 

/* - 


- */ 

*/ 

*/ 

*/ 

*/' 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

V 

*/ 

and  start  */ 
*/ 
*/ 

- */ 


/* Header  and  Include  Files -  > 

/*  */ 

#include  <math.h> 

#include  <stdio.h> 

/* 

/* - Symbolic  Constants -  _  _  */ 


/* - Initialize  Rotating  Base  (Link  1)  Parameters  */ 


double 

Cxi 

— 

0.000, 

/* 

X  distance  from  CS 

to 

CG 

*/ 

double 

Cyl 

= 

0.000, 

/* 

Y  distance  from  CS 

to 

CG 

*/ 

double 

Czl 

= 

0.000, 

/* 

Z  distance  from  CS 

to 

CG 

*/ 

double 

ml 

= 

0.000, 

/* 

mass  */ 

double 

I XX 1 

= 

0.000, 

/* 

Moments  of  Inertia 

*/ 

double 

Iyyl 

= 

145.660, 

double 

Izzl 

= 

0.000, 

double 

Ixyl 

— 

0.000, 

/* 

Products  of  Inertia  * 

/ 

double 

Ixzl 

0.000, 

double 

Iyzl 

= 

0.000, 

/* - Initialize 

Aft  Arm  (Link 

2)  Parameters  */ 

double 

Cx2 

= 

-36.000, 

/* 

X  distance  from  CS 

to 

CG 

*/ 
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double  Cy2  =  0.000;  /*  Y  distance  from  CS  to  CG  */ 

double  Cz2  =  0.000;  /*  Z  distance  from  CS  to  CG  */ 

double  m2  =  0.776;  /*  mass  */ 

double  Ixx2  =  5.830;  /*  Moments  of  Inertia  */ 

double  Iyy2  =  1344.000; 

double  Izz2  =  1344.000; 

double  Ixy2  =  0.000;  /*  Products  of  Inertia  */ 

double  Ixz2  =  0.000; 

double  Iyz2  =  0.000; 

/* - -Initialize  Fore  Arm  (Link  3)  Parameters  */ 

double  Cx3  =  -36.000;  /*  X  distance  from  CS  to  CG  */ 

double  Cy3  =  0.000;  /*  Y  distance  from  CS  to  CG  */ 

double  Cz3  =  0.000;  /*  Z  distance  from  CS  to  CG  */ 

double  m3  =  0.776;  /*  mass  */ 

double  Ixx3  =  5.830;  /*  Moments  of  Inertia  */ 

double  Iyy3  =1344.000; 

double  Izz3  =  1344.000; 

double  Ixy3  =  0.000;  /*  Products  of  Inertia  */ 

double  Ixz3  =  0.000; 

double  Iyz3  =  0.000; 

/* - Initialize  Nozzle  (link  4)  Parameters  */ 

double  Cx4  =  -23.250;  /*  X  distance  from  CS  to  CG  */ 

double  Cy4  =  0.000;  /*  Y  distance  from  CS  to  CG  */ 

double  Cz4  =  0.000;  /*  Z  distance  from  CS  to  CG  */ 

double  m4  =  0.388;  /*  mass  */ 

double  Ixx4  =  2.620;  /*  Moments  of  Inertia  */ 

double  Iyy4  =  339.530; 

double  Izz4  =  339.530; 

double  Ixy4  =  0.000;  /*  Products  of  Inertia  */ 

double  Ixz4  =  0.000; 

double  Iyz4  =  0.000; 

/* - Others  */ 

double  u  [  4 ] ;  /*  Joint  Torque  */ 

/* External  Variables - - - */ 

/*  */ 

/*  Cx . X  distance  from  CS  to  CG  */ 

/*  Cy . Y  distance  from  CS  to  CG  */ 

/*  Cz . Z  distance  from  CS  to  CG  */ 

/*  m . Mass  */ 

/*  Ixx.... . Moment  of  Inertia  */ 

/*  Iyy . .  */ 

/ *  Izz  .  . . . .  */ 

/*  Ixy . Product  of  Inertia  */ 

/*  Ixz .  */ 
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/*  Iyz .  */ 

/*  u . Input  Torques  */ 

/*  */ 

/* - - - */ 


main  () 

{  i 

/* - Passed  Variables - */ 


/*  */ 

/* - */ 

/ 

double  h,  t,  tend,  kl,  k2,  q[4],  qd[4],  y[4],  yd[4]; 
double  pi  =  3.141592654; 
int  m; 

/* - Local  Variables - */ 

/*  */ 

/*  m . Number  of  equations  to  be  integrated  */ 

/*  h . Integration  Step  Size  */ 

/*  t . Simulation  time  */ 

/*  tend . End  Simulations  Time  */ 

/*  kl . Position  Feedback  gain  */ 

/*  k2 . Velocity  Feedback  gain  */ 

/*  q . Joint  Positions  */ 

/*  qd . Joint  Velocities  */ 

/*  pi . Constant  */ 

/*  */ 

/* - */ 

extern  double  sin(),  cos(); 

/* - External  Variables - */ 

/*  */ 

/*  sin . Sine  of  an  Angle  */ 

/*  cos . Cosine  of  an  Angle  */ 

/*  */ 

/* - */ 

m  =  8;  /*  Number  of  equations  to  be  integrated  */ 

h  =  0.05;  /*  Integration  Step  Size  */ 

t  =  0.0;  /*  Initial  Time  */ 

tend  =  5.0;  /*  End  Simulation  Time  */ 

kl  =  64.0;  /*  Position  Error  Feedback  Gain  */ 

k2  =  16.0;  /*  Velocity  Error  Feedback  Gain  */ 

/* - Initial  Position  */ 

y  [  0 ]  =  72.0  *  cos  (  pi*t/10.0  )  +  12.0; 
y  [1]  =  72.0  *  sin  (  pi*t/10.0  )  +  12.0; 
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y [2]  =  -24.75  +  48.0  *  sin  (  pi*t/10.0  )  +  12.0; 
y [3]  =  pi/2.0  +  12 . 0*pi/180 . 0; 

/* - Initial  Global  Velocity  */ 

yd[0]  =  -72 . 0*pi/10 . 0  *  sin  (  pi*t/10.0  ); 

yd[l]  =  72 . 0*pi/l 0 . 0  *  cos  (  pi*t/10.0  ); 

yd[2]  =  48.0*pi/10.0  *  cos  (  pi*t/10.0  ); 

yd [3]  =  0.0; 

/*- — Inverse  Kinematics  to  get  Initial  Joint  Positions  and  Velocities  */ 
inv_kin  (  y,  yd,  q,  qd  )  ; 
control  (  m,  h,  t,  tend,  kl,  k2,  q,  qd  ); 


} 
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/*  matrix:  Matrix  and  Vector  Routines  */ 


mult_matrix  (  a,  m,  n,  b,  p,  c  ) 
double  a [ ] ,  b [ ] ,  c  [  ] ; 
int  m,  n,  p; 

{ 

register  int  i,  j,  k; 
double  x,y; 

for  (  i  =  0;  i  <  m;  i++  ) 
for  (  j  =  0;  j  <  p;  j++  ) 

{ 

x  =  0.0; 

for  (  k  =  0;  k  <  n;  k++  ) 

{  x  =  x  +  * (a+n*i+k)  *  *(b+p*k+j); 

} 

c [p*i+ j ]  =  x; 

} 

} 


smult_matrix  (  a,  m,  n,  b,  s  ) 
double  a [ ] ,  b [ ] ,  s; 
int  m,  n; 

{ 

register  int  i; 

for  (  i  =  0;  i  <  m*n;  i++  ) 
b t i ]  =  * (a+i)  *  s; 

} 

add_matrix  (  a,  m,  n,  b,  c  ) 
double  a [ ] ,  b [ ] ,  c  [  ] ; 
int  m,  n; 

{ 

register  int  i,  j; 

for  (  i  =  0;  i  <m;  i++  ) 
for  (  j  =  0;  j  <  n;  j++  ) 

c [n*i+ j ]  =  * (a+n*i+ j)  +  *(b+n*i+j); 

} 

} 

sub_matrix  (  a,  m,  n,  b,  c  ) 
double  a [ ] ,  b [ ] ,  c [ ] ; 
int  m,  n; 
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{ 

register  int  i,  j; 

for  (  i  =  0;  i  <  m;  i++  ) 

for  (  j  =  0;  j  <  n;  j++  ) 

{ 

c[n*i+j]  =  *  (a+n*i  + j)  -  *(b+n*i+j); 

} 

} 

transpose_matrix  (  a,  m,  n,  at  ) 
double  at]-,  at  [ ]  ; 
int  m,  n; 

{ 

register  int  i,  j; 

for  (  i  =  0;  i  <  m;  i++  ) 

for  (  j  =  0;  j  <  n;  j++  ) 

at[m*j+i]  =  a[n*i+j]; 

} 

print_matrix  (  a,  m,  n  ) 
int  m,  n; 
double  a  [] ; 

{ 

register  int  i,  j; 

for  (  i  =  0;  i  <  m;  i++  ) 

{ 

print f  ("\n"); 

for  (  j  =  0;  j  <  n;  j++  ) 

printf  ("%15.6f",  *(a+n*i+j)  ); 

} 

printf  ("\n") ; 

} 

zero_matrix  (  a,  m,  n  ) 
int  m,  n; 
double  a  [  ] ; 

{ 

register  int  i; 

for  (  i  =  0;  i  <  m*n;  i++  ) 
a[i]  =0.0; 

} 

idenity_matrix  (  a,  m,  n  ) 
int  m,  n; 
double  a  [  ] ; 

{ 

register  int  i; 
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zero_matrix  (a,  m,  n  ) ; 
for  (  i  =  0;  i  <  m*n;  i  =  i+n+1  ) 
a [ i ]  =  1.0; 


cross  (  a,  b,  c  ) 
double  a[3],  b[3],  c[3]; 

{  c [ 0 ]  =  a [1] *b  [2]  -  b[l] *a[2] ; 
c  [  1  ]  =  -  a  [  0 }  *b  1 2  ]  +  b  [  0  ]  *  a  [  2  ] ; 
c [2]  =  a [ 0 ] *b [ 1 ]  -  b[0] *a[l] ; 


double  dot  (  a,  b  ) 
double  a [3] ,  b [3] ; 

return  (  a[0]*b[0]  +  a[l]*b[l]  +  a[2]*b[2]  ); 

} 

double  norm  (  a  ) 
double  a [3]; 

{ 

extern  double  sqrt(); 

return  (  sqrt  (  a[0]*a[0]  +  a[l]*a[l]  +  a[2]*a[2]  )  ); 

} 

unit_vector  (  a,  b  ) 
double  a [ ] ,  b [] ; 

{ 

double  x,  norm  ()  ; 

x  =  norm  (  a  ) ; 
b  [0]  =  a  [ 0 ]  /  x; 
b [  1 ]  =  a[l]  /  x; 
b  [2]  =  a  [2]  /  x; 

} 

print_vector  (  a  ) 
double  a [ 3 ] ; 

{  printf  ("\n%15.6f\n%15.6f\n%15.6f\n",  a[0],  a[l],  a [2 ]  ) 

} 
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/*  output:  Writes  output  data 

/*-- - - - - 

/* 

/*  Written  By:  James  A.  Aardema 
/* 

/*  Date:  November  29,  1988 
„  /* 

/*  Modifications: 

/* 

/*  Called  by:  Control 
/* 

/*  Language:  C 
/* 

/*  Compiler  Options:  None 
/* 

/*  Machine  Dependencies:  None 
/* 

/*  Error:  None 
/* 

/*  Purpose:  Writes  output  data  to  the  screen 
/* 

- - 

/ * - Header  and  Include  Files - 

/* 

/* - Symbolic  Constants - 

/* 

/* - 

output  (  t,  y_d,  y,  yd_d,  yd,  q,  qd,  pos_error,  vel_error,  v,  u 
doublet,  y_d [ 4 ] ,  y[4],  yd_d[4],  yd[4],  q[4],  qd[4]? 
double  pos_error [4] ,  vel_error [4] ,  v[4],  u[4]; 

{ 

/* - Passed  Variables - - 


/* 

/*  yd . .  .  .Desired  global  position 

/*  yT . ....Actual  Position 

/*  yd_d . Desired  global  velcity 

/*  ydT . Actual  Veloctiy 

/*  q. . Joint  Positions 

/*  qd . Joint  Velocity 

/*  pos_error .. .Position  Error  -  (  Desired  -  Actual  ) 
*  /*  vel_error. . .Velocity  Error  -  (  Desired  -  Actual  ) 

/*  v . Control  Input 

/  *  u . Input  Torques 

/* 

/* - 


*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

-*/ 

-*/ 

*/ 

-*/ 

*/ 

-*/ 


*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 
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/* - Local  Variables 

/* 

/* - 


*/ 

*/ 

*/ 


/* - External  Variables 

/* 

/* - 


*/ 

*/ 

*/ 


printf 

r\n")  ; 

printf 

("Time  =  %12.5f\n",  t 

) ; 

printf 

("y  d  =  %12.5f 

%12.5f 

%12.5f 

%12.5f\n", 

y_d [  0  ] , 

y_d [  l  ] , 

y_d [2] , 

y_d [ 3 ]  )  ; 

printf 

("y  =  %12.5f 

%12.5f 

%12.5f 

%12.5f\n", 

y  [0] , 

y  ti]  / 

y  [2] , 

y [3]  )/ 

printf 

("pos  err=  %12.5f 

%12.5f 

%12.5f 

%12 . 5f \n", 

pos_error [0] , 

pos  error[l],  pos_error [2 ) , 

pos  error [3]  )  ; 

printf 

("yd  d  =  %12.5f 

%12.5f 

%12.5f 

%12.5f\n". 

yd_d [ 0 ] 

r 

yd_d [ 1 ] ,  yd_d[2]. 

yd_d [3]  )  ; 

printf 

("yd  =  %12.5f 

%12.5f 

%12.5f 

%12.5f\n". 

yd [0] , 

yd[l] , 

yd  [  2  ]  , 

yd [3]  ) ; 

printf 

("vel  err=  %12.5f 

%12.5f 

%12.5f 

%12 . 5f\n", 

vel_error [0] , 

vel  error[l],  vel_error [2 ] , 

vel_error[3]  )/ 

printf 

("q  =  %12.5f 

%12.5f 

%12.5f 

%12.5f\n", 

q  [  o  ] , 

q  [  i  ] , 

q[2]  , 

q [3]  )  ; 

printf 

(”qd  =  %12.5f 

%12.5f 

%12.5f 

%12.5f\n". 

qd [ o ] , 

qd [ i ] / 

qd [2] , 

qd [ 3 ]  ); 

printf 

("v  =  %12.5f 

%12.5f 

%12.5f 

%12.5f\n". 

v  [  0  ]  , 

V(l], 

v [2 ) , 

v [3]  )  ; 

printf 

("u  =  %12.5f 

%12.5f 

%12.5f 

%12.5f\n". 

u[0]. 

u[l] , 

u  [2 ]  , 

u  [  3  ]  ); 
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/*  plotl:  Writes  output  data  for  plotting 


y 


P 


/*- - 

/* 

/*  Written  By:  James  A.  Aardema 
/* 

/*  Date:  November  29,  1988 
/* 

/*  Modifications: 

/* 

/*  Called  by:  Control 
/* 

/*  Language:  C 
/* 

/*  Compiler  Options:  None 
/* 

/*  Machine  Dependencies:  None 
/* 

/*  Error:  None 
/* 

/*  Purpose:  Writes  output  data  to  the  screen  in  columns  for  plotting 
/* 

/* - - - 


*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

•*/ 


/* - Header  and  Include  Files - 

/* 

/* - Symbolic  Constants - 

/* 

/*__ - 

plotl  (  t,  y_d,  y,  yd_d,  yd,  q,  qd,  pos_error,  vel_error,  v,  u 
doublet,  y_d [ 4 ] ,  y[4],  yd_d[4],  yd [4],  q[4],  qd[4]; 
double  pos_error [4] ,  vel_error [4 ] ,  v[4],  u[4]; 

{ 

/* - Passed  Variables - 


/* 

/*  yd . Desired  global  position 

/*  yT . Actual  Position 

/*  yd_d . Desired  global  velcity 

/*  yd . Actual  Veloctiy 

/*  q . Joint  Positions 

/*  qd . Joint  Velocity 

/*  pos  error ...  Posit ion  Error  -  (  Desired  -  Actual  ) 
*  /*  vel~error .. .Velocity  Error  -  (  Desired  -  Actual  ) 

/*  v . Control  Input 

/*  u . Input  Torques 

/* 

/* - 


*/ 

*/ 

*/ 

*/ 

*/ 


*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 
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double  pi  =  3.141592654; 


/* - Local  Variables - 

/* 

/*  pi . Constant 

/* 

/* - 

/* - External  Variables - 

/* 

/* - 

print f  ("%5.3f",  t  ); 

printf  ("  %8.4f  %8.4f  %8.4f  %8.4f", 

y_d [  0  ] ,  y_d [ 1 ] ,  y_d(2] ,  y_d[3] *180. 0/pi  ); 

printf  ("  %8.4f  %8.4f  %8.4f  %8.4f", 

y [0] ,  y [1] ,  y  [2] ,  y[3] *180. 0/pi  ); 


*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 


printf  ("  %8.4f  %8.4f  %8.4f  %8.4f", 

pos_error [0] ,  pos_error [ 1 ] ,  pos_error [2 ] ,  pos_error [3] *180 . 0/pi  ); 

printf  ("  %8.4f  %8.4f  %8.4f  %8.4f", 

vel_error [0] ,  vel_error [ 1 ] ,  vel_error [2 ] ,  vel_error [3] *180 . 0/pi  ); 

printf  ("\n”) ; 


} 
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/*  plot2:  Writes  output  data  for  plotting 

/* - - -  — — - 

/* 

/*  Written  By:  James  A.  Aardema 
/* 

/*  Date:  November  29,  1988 
/* 

/*  Modifications: 

/* 

/*  Called  by:  Control 
/* 

/*  Language:  C 
/* 

/*  Compiler  Options:  None 
/* 

/*  Machine  Dependencies:  None 
/* 

/*  Error:  None 

/*  '  , 
j *  Purpose:  Writes  output  data  to  the  screen  m  columns  for  plotting 

/* 

/* - — - 


/* - Header  and  Include  Files 

/* 

/* - Symbolic  Constants - 


/* 

/* 


*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

-*/ 

-*/ 

*/ 

-*/ 

*/ 

-*/ 


plot2  (  t,  y_d,  y,  yd_d,  yd,  q,  qd,  pos_error,  vel_error,  v,  u  ) 
double  t,  y_d [ 4 ] ,  y[4],  yd_d[4],  yd [ 4 ] ,  q[4],  qd[4]; 
double  pos_error [4 ] ,  vel_error [4] ,  v[4],  u [ 4 ] ; 

{ 

/ * - Passed  Variables - 


/* 

./*  y  d . Desired  global  position 

/*  y7 . Actual  Position 

/*  yd_d . Desired  global  velcity 

/*  yd . Actual  Veloctiy 

/*  q . Joint  Positions 

/*  qd . Joint  Velocity 

/*  pos_error. . .Position  Error  -  (  Desired  -  Actual  ) 
/*  vel_error. . .Velocity  Error  -  (  Desired  -  Actual  ) 

/*  . Control  Input 

/*  . Input  Torques 

/* 


*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 


double  pi  =  3.141592654; 


/* - Local  Variables - 

/* 

/*  pi . Constant 

/* 

/* - 

/* - External  Variables - 

/* 

/* - 

printf  ("%5.3f",  t  ); 

printf  ("  %6.4f  %6.4f  %6.4f  %6.4f", 

q[0],  q[l],  q [ 2 ] ,  q ( 3 ]  ); 

printf  ("  %8.4f  %8.4f  %8.4f  %8.4f", 

qd[0],  qd [ 1 ] ,  qd [ 2 ] ,  qd[3J  ); 

printf  (”  %8.3f  %8.3f  %8.3f  %8.3f',/ 

v[0],  v[l],  v [2 ] ,  v [ 3]  )  ; 

printf  ("  %8.3f  %8.3f  %8.3f  %8.3f", 

u[0],  u[l],  u[2],  u [3]  ) ; 

printf  ("\n") ; 

} 


*// 

*/ 

*/ 

*/ 

*/ 

*/ 
*/  ’ 
*/ 
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1* 


p 


4 


/*  rk4  step:  Integrate  a  system  of  first  order  differential  equations 
/*  “  over  1  time  step  using  4th  order  Runga-Kutta 


/*  Written  By:  James  A.  Aardema 
/* 

/*  Date:  October  10,  1988 
/* 

/*  Modifications: 

/* 

/*  Called  by: 

/* 

/*  Language:  C 

7* 

/*  Compiler  Options:  None 
/* 

/*  Machine  Dependencies: 

/* 

/*  Error: 

/*  Purpose:  This  subroutine  integrates  a  system  of  "m"  first  order 
/*  initial  value  problems  using  a  4th  order  Runga-Kutta  Algorithm. 

/*  The  system  of  equations  is  integrated  form  (t)  to  (t+h) .  The  values 
/*  t  is  updated  upon  return.  The  vector  "y"  contains  the  initial 
/*  conditions  upon  entry  and  contains  the  final  values  upon  output. 

/*  The  routine  is  designed  to  be  called  repeatedly,  in  a  for  or  while 
/*  loop  with  with  minimum  effort. 

/* 

/*  A  working  vector  "w"  of  length  (  5  *  m  )  is  required. 

/* 

/*  For  a  complete  description  on  the  theory  and  development  of  this 
/*  algorithm  see  page  264  of  the  book  "Numerical  Analysis  by 
/*  Richard  L.  Burden  and  J.  Douglas  Faires. 

/* 

- - 

/* - Header  and  Include  Files - 

/* 

/* - Symbolic  Constants - 

/* 

- - 


*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

-*/ 

-*/ 

*/ 

-*/ 

*/ 

-*/ 


rk4_step  (  m,  y,  w,  h,  t  ) 
double  y [ ] ,  w[] ,  *h,  *t; 
int  m; 

{ 

/* - Passed  Variables - 
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/*  */ 

/*  m . Number  of  equations  to  be  Integrated  */ 

/*  y . Initial  Conditions  on  input;  Final  conditions  on  Output  */ 

/*  w . Working  vector  of  length  "5*m”  */ 

/*  h . Integration  Step  Size  */ 

/*  t . Current  Integration  Time  */ 

/*  */ 

/* - */ 

double  x,  *kl,  *k2,  *k3,  *k4; 
register  int  j; 

* 

/* - Local  Variables - */ 

/*  */ 

/*  x . Local  Value  for  Time  */ 

/*  kl . Working  Vector  */ 

/*  k2 . Working  Vector  */ 

/*  k3 . Working  Vector  */ 

/*  k4 . Working  Vector  */ 

/*  j . Index  */ 

/*  */ 

/* - - - */ 

/* - External  Variables - - - */ 

/*  */ 

/* - */ 

kl  =  w  +  m;  /*  Set  working  vector  of  lenght  "m"  */ 

k2  =  kl  +  m;  /*  Set  working  vector  of  length  "m"  */ 

k3  =  k2  +  m;  /*  Set  working  vector  of  lenght  "m"  */ 

k4  =  k3  +  m;  /*  Set  working  vector  of  lenght  "m”  */ 

/*  Step  5  */ 

diffeq  (  kl,  *t,  y,  m  ); 

/*  Step  6  */ 
x  =  *t  +  *h/2.0; 

for  (  j  =  0;  j  <  m;  j++  )  w [ j ]  =  y[j]  +  *h*kl  [  j] / 2 . 0 ; 
diffeq  (  k2,  x,  w,  m  ) ; 

/*  Step  7  */ 

for  (  j  =  0;  j  <  m;  j++  )  w[j]  =  y [ j ]  +  *h*k2 [ j ] /2 . 0; 
diffeq  (  k3,  x,  w,  m  ) ; 

/*  Step  8  */ 

x  =  *t  +  *h; 

for  (  j  =  0;  j  <  m;  j++  )  w [ j ]  =  y [ j ]  +  *h*k3[j]; 
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diffeq  (  k4,  x,  w,  m  ); 
/*  Step  9  */ 


for  (  j  =  0;  j  <  m;  j++ 

ytj]  =  y [ j]  +  *h*(  kl [ j]  +  k2 [ j]  +  k2[j]  +  k3 1 D ] 


+  k3[j] 


/*  Step  10  */ 


*t  =  *t  +  *h; 


k4  [  j ] )  /  6.0 
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/*  trajectory:  Defines  the  robot  trajectory 


*/ 


/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 


Written  By:  James  A.  Aardema 
Date:  November  29,  1988 

Modifications : 

Called  by: 

Language:  C 

Compiler  Options:  None 
Machine  Dependencies:  None 
Error:  None 

Purpose:  Define  the  trajectory  of  the  nozzle  (  Coordinate  System  4  ) 


*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/' 

*/ 

*/. 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 

*/ 


/* - Header  and  Include  Files 

/* 


/* - Symbolic  Constants 

/* 

/* - 


*/ 

*/ 

*/ 

*/ 

*/ 


trajectory  (  t,  y_d,  yd_d,  ydd_d  ) 
double  t,  y_d[4],  yd_d[4],  ydd_d[4]; 
{ 


/* - Passed  Variables 


/* 

/*  t . Time 

/*  y_d. . Desired  Position 

/*  yd_d . Desired  Velocity 

./*  ydd_d . Desired  Accelerations 

/* 

/* - 


*/ 

*/ 

*/ 

*/ 

*/ 

*/_ 

*/ 

*/ 


double  x; 

double  pi  =  3.141592654; 

/* Local  Variables - */ 

/*  */ 
/*  x . Scaled  Value  of  Time  */ 
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/*  pi . Constant 

/* 

- 

extern  double  sin(),  cos(); 
/* - External  Variables - 


/* 

/*  sin. . Sine  of  an  angle 

/*  cos. . Cosine  of  an  angle 

/* 

/* - 


/* - Desired  Trajectory  Position  */ 

y  d [ 0 ]  =  72.0  *  cos  (  pi*t/10.0  ); 

y  d 1 1 3  -  72.0  *  sin  (  pi*t/10.0  ); 

y_d [2 ]  =  -24.75  +  48.0  *  sin  (  pi*t/10.0  ); 
y~d [ 3 ]  =  pi/2.0; 

/* - Desired  Trajectory  Velocity  */ 

yd  d 1 0 ]  =  -72 . 0*pi/10 . 0  *  sin  (  pi*t/10.0  ); 

yd  d [ 1 3  =  72 . 0*pi/10 . 0  *  cos  (  pi*t/10.0  ); 

y d_d [ 2 ]  =  48 . 0*pi/10 . 0  *  cos  (  pi*t/10.0  ); 

yd_d [ 3 ]  =  0.0; 

/* - Desired  Trajectory  Acceleration  */ 

ydd  d[0]  =  -72.0*pi*pi/100.0  *  cos  (  pi*t/10.0  ); 

ydd  d[l]  =  -72.0*pi*pi/100.0  *  sin  (  pi*t/10.0  ); 

ydd_d [ 2 ]  =  -48 . 0*pi*pi/100 . 0  *  sin  (  pi*t/10.0  ); 

ydd_d [ 3 ]  =  0.0; 


*/ 

*/ 

*/ 


*/ 

*/ 

*/ 

*/ 

*/ 

*/ 
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